From 57b26697e1182d485f028090656b49e3733d1cc4 Mon Sep 17 00:00:00 2001 From: klaxalk Date: Thu, 25 Apr 2024 05:43:02 +0000 Subject: [PATCH] deploy: 93ca382a2ba32dcd171dc3042d5831f013aec01c --- .nojekyll | 0 amber.png | Bin 0 -> 141 bytes badge.svg | 1 + emerald.png | Bin 0 -> 141 bytes gcov.css | 519 + glass.png | Bin 0 -> 167 bytes index-sort-f.html | 612 ++ index-sort-l.html | 612 ++ index.html | 612 ++ .../attitude_converter.h.func-sort-c.html | 124 + .../mrs_lib/attitude_converter.h.func.html | 124 + .../attitude_converter.h.gcov.frameset.html | 19 + .../mrs_lib/attitude_converter.h.gcov.html | 617 ++ .../attitude_converter.h.gcov.overview.html | 154 + .../mrs_lib/attitude_converter.h.gcov.png | Bin 0 -> 1675 bytes ...dynamic_reconfigure_mgr.h.func-sort-c.html | 420 + .../dynamic_reconfigure_mgr.h.func.html | 420 + ...namic_reconfigure_mgr.h.gcov.frameset.html | 19 + .../dynamic_reconfigure_mgr.h.gcov.html | 343 + ...namic_reconfigure_mgr.h.gcov.overview.html | 85 + .../dynamic_reconfigure_mgr.h.gcov.png | Bin 0 -> 1370 bytes .../geometry/cyclic.h.func-sort-c.html | 548 + .../mrs_lib/geometry/cyclic.h.func.html | 548 + .../geometry/cyclic.h.gcov.frameset.html | 19 + .../mrs_lib/geometry/cyclic.h.gcov.html | 612 ++ .../geometry/cyclic.h.gcov.overview.html | 152 + .../mrs_lib/geometry/cyclic.h.gcov.png | Bin 0 -> 2145 bytes .../mrs_lib/geometry/index-detail-sort-f.html | 128 + .../mrs_lib/geometry/index-detail-sort-l.html | 128 + .../mrs_lib/geometry/index-detail.html | 128 + .../mrs_lib/geometry/index-sort-f.html | 112 + .../mrs_lib/geometry/index-sort-l.html | 112 + mrs_lib/include/mrs_lib/geometry/index.html | 112 + .../mrs_lib/geometry/misc.h.func-sort-c.html | 92 + .../include/mrs_lib/geometry/misc.h.func.html | 92 + .../geometry/misc.h.gcov.frameset.html | 19 + .../include/mrs_lib/geometry/misc.h.gcov.html | 408 + .../geometry/misc.h.gcov.overview.html | 101 + .../include/mrs_lib/geometry/misc.h.gcov.png | Bin 0 -> 1071 bytes .../gps_conversions.h.func-sort-c.html | 96 + .../mrs_lib/gps_conversions.h.func.html | 96 + .../gps_conversions.h.gcov.frameset.html | 19 + .../mrs_lib/gps_conversions.h.gcov.html | 387 + .../gps_conversions.h.gcov.overview.html | 96 + .../mrs_lib/gps_conversions.h.gcov.png | Bin 0 -> 1579 bytes .../mrs_lib/impl/index-detail-sort-f.html | 236 + .../mrs_lib/impl/index-detail-sort-l.html | 236 + .../include/mrs_lib/impl/index-detail.html | 236 + .../include/mrs_lib/impl/index-sort-f.html | 172 + .../include/mrs_lib/impl/index-sort-l.html | 172 + mrs_lib/include/mrs_lib/impl/index.html | 172 + .../impl/param_provider.hpp.func-sort-c.html | 144 + .../mrs_lib/impl/param_provider.hpp.func.html | 144 + .../param_provider.hpp.gcov.frameset.html | 19 + .../mrs_lib/impl/param_provider.hpp.gcov.html | 132 + .../param_provider.hpp.gcov.overview.html | 32 + .../mrs_lib/impl/param_provider.hpp.gcov.png | Bin 0 -> 347 bytes .../publisher_handler.hpp.func-sort-c.html | 904 ++ .../impl/publisher_handler.hpp.func.html | 904 ++ .../publisher_handler.hpp.gcov.frameset.html | 19 + .../impl/publisher_handler.hpp.gcov.html | 332 + .../publisher_handler.hpp.gcov.overview.html | 82 + .../impl/publisher_handler.hpp.gcov.png | Bin 0 -> 798 bytes ...ervice_client_handler.hpp.func-sort-c.html | 268 + .../impl/service_client_handler.hpp.func.html | 268 + ...vice_client_handler.hpp.gcov.frameset.html | 19 + .../impl/service_client_handler.hpp.gcov.html | 403 + ...vice_client_handler.hpp.gcov.overview.html | 100 + .../impl/service_client_handler.hpp.gcov.png | Bin 0 -> 969 bytes .../subscribe_handler.hpp.func-sort-c.html | 4256 ++++++++ .../impl/subscribe_handler.hpp.func.html | 4256 ++++++++ .../subscribe_handler.hpp.gcov.frameset.html | 19 + .../impl/subscribe_handler.hpp.gcov.html | 413 + .../subscribe_handler.hpp.gcov.overview.html | 103 + .../impl/subscribe_handler.hpp.gcov.png | Bin 0 -> 1372 bytes .../mrs_lib/impl/timer.hpp.func-sort-c.html | 92 + .../include/mrs_lib/impl/timer.hpp.func.html | 92 + .../mrs_lib/impl/timer.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/timer.hpp.gcov.html | 180 + .../mrs_lib/impl/timer.hpp.gcov.overview.html | 44 + .../include/mrs_lib/impl/timer.hpp.gcov.png | Bin 0 -> 532 bytes .../impl/transformer.hpp.func-sort-c.html | 272 + .../mrs_lib/impl/transformer.hpp.func.html | 272 + .../impl/transformer.hpp.gcov.frameset.html | 19 + .../mrs_lib/impl/transformer.hpp.gcov.html | 280 + .../impl/transformer.hpp.gcov.overview.html | 69 + .../mrs_lib/impl/transformer.hpp.gcov.png | Bin 0 -> 841 bytes .../mrs_lib/impl/ukf.hpp.func-sort-c.html | 128 + .../include/mrs_lib/impl/ukf.hpp.func.html | 128 + .../mrs_lib/impl/ukf.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/ukf.hpp.gcov.html | 364 + .../mrs_lib/impl/ukf.hpp.gcov.overview.html | 90 + mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png | Bin 0 -> 1198 bytes .../vector_converter.hpp.func-sort-c.html | 152 + .../impl/vector_converter.hpp.func.html | 152 + .../vector_converter.hpp.gcov.frameset.html | 19 + .../impl/vector_converter.hpp.gcov.html | 173 + .../vector_converter.hpp.gcov.overview.html | 43 + .../impl/vector_converter.hpp.gcov.png | Bin 0 -> 456 bytes .../include/mrs_lib/index-detail-sort-f.html | 444 + .../include/mrs_lib/index-detail-sort-l.html | 444 + mrs_lib/include/mrs_lib/index-detail.html | 444 + mrs_lib/include/mrs_lib/index-sort-f.html | 292 + mrs_lib/include/mrs_lib/index-sort-l.html | 292 + mrs_lib/include/mrs_lib/index.html | 292 + .../include/mrs_lib/lkf.h.func-sort-c.html | 268 + mrs_lib/include/mrs_lib/lkf.h.func.html | 268 + .../include/mrs_lib/lkf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/lkf.h.gcov.html | 460 + .../include/mrs_lib/lkf.h.gcov.overview.html | 114 + mrs_lib/include/mrs_lib/lkf.h.gcov.png | Bin 0 -> 1750 bytes .../mrs_lib/msg_extractor.h.func-sort-c.html | 256 + .../include/mrs_lib/msg_extractor.h.func.html | 256 + .../msg_extractor.h.gcov.frameset.html | 19 + .../include/mrs_lib/msg_extractor.h.gcov.html | 775 ++ .../msg_extractor.h.gcov.overview.html | 193 + .../include/mrs_lib/msg_extractor.h.gcov.png | Bin 0 -> 1287 bytes .../include/mrs_lib/mutex.h.func-sort-c.html | 400 + mrs_lib/include/mrs_lib/mutex.h.func.html | 400 + .../mrs_lib/mutex.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/mutex.h.gcov.html | 260 + .../mrs_lib/mutex.h.gcov.overview.html | 64 + mrs_lib/include/mrs_lib/mutex.h.gcov.png | Bin 0 -> 739 bytes .../mrs_lib/param_loader.h.func-sort-c.html | 392 + .../include/mrs_lib/param_loader.h.func.html | 392 + .../mrs_lib/param_loader.h.gcov.frameset.html | 19 + .../include/mrs_lib/param_loader.h.gcov.html | 1452 +++ .../mrs_lib/param_loader.h.gcov.overview.html | 362 + .../include/mrs_lib/param_loader.h.gcov.png | Bin 0 -> 4406 bytes .../publisher_handler.h.func-sort-c.html | 556 ++ .../mrs_lib/publisher_handler.h.func.html | 556 ++ .../publisher_handler.h.gcov.frameset.html | 19 + .../mrs_lib/publisher_handler.h.gcov.html | 256 + .../publisher_handler.h.gcov.overview.html | 63 + .../mrs_lib/publisher_handler.h.gcov.png | Bin 0 -> 572 bytes ...uadratic_throttle_model.h.func-sort-c.html | 88 + .../quadratic_throttle_model.h.func.html | 88 + ...dratic_throttle_model.h.gcov.frameset.html | 19 + .../quadratic_throttle_model.h.gcov.html | 117 + ...dratic_throttle_model.h.gcov.overview.html | 29 + .../quadratic_throttle_model.h.gcov.png | Bin 0 -> 246 bytes .../mrs_lib/repredictor.h.func-sort-c.html | 256 + .../include/mrs_lib/repredictor.h.func.html | 256 + .../mrs_lib/repredictor.h.gcov.frameset.html | 19 + .../include/mrs_lib/repredictor.h.gcov.html | 637 ++ .../mrs_lib/repredictor.h.gcov.overview.html | 159 + .../include/mrs_lib/repredictor.h.gcov.png | Bin 0 -> 2387 bytes .../safety_zone/index-detail-sort-f.html | 112 + .../safety_zone/index-detail-sort-l.html | 112 + .../mrs_lib/safety_zone/index-detail.html | 112 + .../mrs_lib/safety_zone/index-sort-f.html | 112 + .../mrs_lib/safety_zone/index-sort-l.html | 112 + .../include/mrs_lib/safety_zone/index.html | 112 + .../safety_zone/polygon.h.func-sort-c.html | 92 + .../mrs_lib/safety_zone/polygon.h.func.html | 92 + .../safety_zone/polygon.h.gcov.frameset.html | 19 + .../mrs_lib/safety_zone/polygon.h.gcov.html | 136 + .../safety_zone/polygon.h.gcov.overview.html | 33 + .../mrs_lib/safety_zone/polygon.h.gcov.png | Bin 0 -> 337 bytes .../safety_zone.h.func-sort-c.html | 84 + .../safety_zone/safety_zone.h.func.html | 84 + .../safety_zone.h.gcov.frameset.html | 19 + .../safety_zone/safety_zone.h.gcov.html | 121 + .../safety_zone.h.gcov.overview.html | 30 + .../safety_zone/safety_zone.h.gcov.png | Bin 0 -> 280 bytes .../mrs_lib/scope_timer.h.func-sort-c.html | 92 + .../include/mrs_lib/scope_timer.h.func.html | 92 + .../mrs_lib/scope_timer.h.gcov.frameset.html | 19 + .../include/mrs_lib/scope_timer.h.gcov.html | 248 + .../mrs_lib/scope_timer.h.gcov.overview.html | 61 + .../include/mrs_lib/scope_timer.h.gcov.png | Bin 0 -> 777 bytes .../service_client_handler.h.func-sort-c.html | 176 + .../service_client_handler.h.func.html | 176 + ...ervice_client_handler.h.gcov.frameset.html | 19 + .../service_client_handler.h.gcov.html | 327 + ...ervice_client_handler.h.gcov.overview.html | 81 + .../mrs_lib/service_client_handler.h.gcov.png | Bin 0 -> 689 bytes .../subscribe_handler.h.func-sort-c.html | 3044 ++++++ .../mrs_lib/subscribe_handler.h.func.html | 3044 ++++++ .../subscribe_handler.h.gcov.frameset.html | 19 + .../mrs_lib/subscribe_handler.h.gcov.html | 548 + .../subscribe_handler.h.gcov.overview.html | 136 + .../mrs_lib/subscribe_handler.h.gcov.png | Bin 0 -> 1783 bytes .../timeout_manager.h.func-sort-c.html | 80 + .../mrs_lib/timeout_manager.h.func.html | 80 + .../timeout_manager.h.gcov.frameset.html | 19 + .../mrs_lib/timeout_manager.h.gcov.html | 169 + .../timeout_manager.h.gcov.overview.html | 42 + .../mrs_lib/timeout_manager.h.gcov.png | Bin 0 -> 435 bytes .../include/mrs_lib/timer.h.func-sort-c.html | 108 + mrs_lib/include/mrs_lib/timer.h.func.html | 108 + .../mrs_lib/timer.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/timer.h.gcov.html | 355 + .../mrs_lib/timer.h.gcov.overview.html | 88 + mrs_lib/include/mrs_lib/timer.h.gcov.png | Bin 0 -> 935 bytes .../mrs_lib/transformer.h.func-sort-c.html | 152 + .../include/mrs_lib/transformer.h.func.html | 152 + .../mrs_lib/transformer.h.gcov.frameset.html | 19 + .../include/mrs_lib/transformer.h.gcov.html | 764 ++ .../mrs_lib/transformer.h.gcov.overview.html | 190 + .../include/mrs_lib/transformer.h.gcov.png | Bin 0 -> 2328 bytes .../include/mrs_lib/ukf.h.func-sort-c.html | 88 + mrs_lib/include/mrs_lib/ukf.h.func.html | 88 + .../include/mrs_lib/ukf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/ukf.h.gcov.html | 285 + .../include/mrs_lib/ukf.h.gcov.overview.html | 71 + mrs_lib/include/mrs_lib/ukf.h.gcov.png | Bin 0 -> 1009 bytes .../include/mrs_lib/utils.h.func-sort-c.html | 100 + mrs_lib/include/mrs_lib/utils.h.func.html | 100 + .../mrs_lib/utils.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/utils.h.gcov.html | 229 + .../mrs_lib/utils.h.gcov.overview.html | 57 + mrs_lib/include/mrs_lib/utils.h.gcov.png | Bin 0 -> 677 bytes .../vector_converter.h.func-sort-c.html | 224 + .../mrs_lib/vector_converter.h.func.html | 224 + .../vector_converter.h.gcov.frameset.html | 19 + .../mrs_lib/vector_converter.h.gcov.html | 128 + .../vector_converter.h.gcov.overview.html | 31 + .../mrs_lib/vector_converter.h.gcov.png | Bin 0 -> 366 bytes .../mrs_lib/visual_object.h.func-sort-c.html | 84 + .../include/mrs_lib/visual_object.h.func.html | 84 + .../visual_object.h.gcov.frameset.html | 19 + .../include/mrs_lib/visual_object.h.gcov.html | 186 + .../visual_object.h.gcov.overview.html | 46 + .../include/mrs_lib/visual_object.h.gcov.png | Bin 0 -> 493 bytes .../attitude_converter.cpp.func-sort-c.html | 244 + .../attitude_converter.cpp.func.html | 244 + .../attitude_converter.cpp.gcov.frameset.html | 19 + .../attitude_converter.cpp.gcov.html | 561 ++ .../attitude_converter.cpp.gcov.overview.html | 140 + .../attitude_converter.cpp.gcov.png | Bin 0 -> 1533 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../src/attitude_converter/index-detail.html | 110 + .../src/attitude_converter/index-sort-f.html | 102 + .../src/attitude_converter/index-sort-l.html | 102 + mrs_lib/src/attitude_converter/index.html | 102 + .../batch_visualizer.cpp.func-sort-c.html | 172 + .../batch_visualizer.cpp.func.html | 172 + .../batch_visualizer.cpp.gcov.frameset.html | 19 + .../batch_visualizer.cpp.gcov.html | 440 + .../batch_visualizer.cpp.gcov.overview.html | 109 + .../batch_visualizer.cpp.gcov.png | Bin 0 -> 1265 bytes .../batch_visualizer/index-detail-sort-f.html | 128 + .../batch_visualizer/index-detail-sort-l.html | 128 + .../src/batch_visualizer/index-detail.html | 128 + .../src/batch_visualizer/index-sort-f.html | 112 + .../src/batch_visualizer/index-sort-l.html | 112 + mrs_lib/src/batch_visualizer/index.html | 112 + .../visual_object.cpp.func-sort-c.html | 168 + .../visual_object.cpp.func.html | 168 + .../visual_object.cpp.gcov.frameset.html | 19 + .../visual_object.cpp.gcov.html | 435 + .../visual_object.cpp.gcov.overview.html | 108 + .../visual_object.cpp.gcov.png | Bin 0 -> 1229 bytes .../geometry/conversions.cpp.func-sort-c.html | 120 + .../src/geometry/conversions.cpp.func.html | 120 + .../conversions.cpp.gcov.frameset.html | 19 + .../src/geometry/conversions.cpp.gcov.html | 178 + .../conversions.cpp.gcov.overview.html | 44 + mrs_lib/src/geometry/conversions.cpp.gcov.png | Bin 0 -> 446 bytes mrs_lib/src/geometry/index-detail-sort-f.html | 138 + mrs_lib/src/geometry/index-detail-sort-l.html | 138 + mrs_lib/src/geometry/index-detail.html | 138 + mrs_lib/src/geometry/index-sort-f.html | 122 + mrs_lib/src/geometry/index-sort-l.html | 122 + mrs_lib/src/geometry/index.html | 122 + .../src/geometry/misc.cpp.func-sort-c.html | 144 + mrs_lib/src/geometry/misc.cpp.func.html | 144 + .../src/geometry/misc.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/misc.cpp.gcov.html | 269 + .../src/geometry/misc.cpp.gcov.overview.html | 67 + mrs_lib/src/geometry/misc.cpp.gcov.png | Bin 0 -> 747 bytes .../src/geometry/shapes.cpp.func-sort-c.html | 352 + mrs_lib/src/geometry/shapes.cpp.func.html | 352 + .../geometry/shapes.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/shapes.cpp.gcov.html | 730 ++ .../geometry/shapes.cpp.gcov.overview.html | 182 + mrs_lib/src/geometry/shapes.cpp.gcov.png | Bin 0 -> 1596 bytes mrs_lib/src/math/index-detail-sort-f.html | 110 + mrs_lib/src/math/index-detail-sort-l.html | 110 + mrs_lib/src/math/index-detail.html | 110 + mrs_lib/src/math/index-sort-f.html | 102 + mrs_lib/src/math/index-sort-l.html | 102 + mrs_lib/src/math/index.html | 102 + mrs_lib/src/math/math.cpp.func-sort-c.html | 84 + mrs_lib/src/math/math.cpp.func.html | 84 + mrs_lib/src/math/math.cpp.gcov.frameset.html | 19 + mrs_lib/src/math/math.cpp.gcov.html | 149 + mrs_lib/src/math/math.cpp.gcov.overview.html | 37 + mrs_lib/src/math/math.cpp.gcov.png | Bin 0 -> 388 bytes .../median_filter/index-detail-sort-f.html | 110 + .../median_filter/index-detail-sort-l.html | 110 + mrs_lib/src/median_filter/index-detail.html | 110 + mrs_lib/src/median_filter/index-sort-f.html | 102 + mrs_lib/src/median_filter/index-sort-l.html | 102 + mrs_lib/src/median_filter/index.html | 102 + .../median_filter.cpp.func-sort-c.html | 148 + .../median_filter/median_filter.cpp.func.html | 148 + .../median_filter.cpp.gcov.frameset.html | 19 + .../median_filter/median_filter.cpp.gcov.html | 286 + .../median_filter.cpp.gcov.overview.html | 71 + .../median_filter/median_filter.cpp.gcov.png | Bin 0 -> 872 bytes .../src/param_loader/index-detail-sort-f.html | 110 + .../src/param_loader/index-detail-sort-l.html | 110 + mrs_lib/src/param_loader/index-detail.html | 110 + mrs_lib/src/param_loader/index-sort-f.html | 102 + mrs_lib/src/param_loader/index-sort-l.html | 102 + mrs_lib/src/param_loader/index.html | 102 + .../param_provider.cpp.func-sort-c.html | 96 + .../param_loader/param_provider.cpp.func.html | 96 + .../param_provider.cpp.gcov.frameset.html | 19 + .../param_loader/param_provider.cpp.gcov.html | 200 + .../param_provider.cpp.gcov.overview.html | 49 + .../param_loader/param_provider.cpp.gcov.png | Bin 0 -> 659 bytes mrs_lib/src/profiler/index-detail-sort-f.html | 110 + mrs_lib/src/profiler/index-detail-sort-l.html | 110 + mrs_lib/src/profiler/index-detail.html | 110 + mrs_lib/src/profiler/index-sort-f.html | 102 + mrs_lib/src/profiler/index-sort-l.html | 102 + mrs_lib/src/profiler/index.html | 102 + .../profiler/profiler.cpp.func-sort-c.html | 120 + mrs_lib/src/profiler/profiler.cpp.func.html | 120 + .../profiler/profiler.cpp.gcov.frameset.html | 19 + mrs_lib/src/profiler/profiler.cpp.gcov.html | 301 + .../profiler/profiler.cpp.gcov.overview.html | 75 + mrs_lib/src/profiler/profiler.cpp.gcov.png | Bin 0 -> 829 bytes .../src/safety_zone/index-detail-sort-f.html | 128 + .../src/safety_zone/index-detail-sort-l.html | 128 + mrs_lib/src/safety_zone/index-detail.html | 128 + mrs_lib/src/safety_zone/index-sort-f.html | 112 + mrs_lib/src/safety_zone/index-sort-l.html | 112 + mrs_lib/src/safety_zone/index.html | 112 + .../line_operations.cpp.func-sort-c.html | 92 + .../safety_zone/line_operations.cpp.func.html | 92 + .../line_operations.cpp.gcov.frameset.html | 19 + .../safety_zone/line_operations.cpp.gcov.html | 143 + .../line_operations.cpp.gcov.overview.html | 35 + .../safety_zone/line_operations.cpp.gcov.png | Bin 0 -> 465 bytes .../polygon/index-detail-sort-f.html | 110 + .../polygon/index-detail-sort-l.html | 110 + .../src/safety_zone/polygon/index-detail.html | 110 + .../src/safety_zone/polygon/index-sort-f.html | 102 + .../src/safety_zone/polygon/index-sort-l.html | 102 + mrs_lib/src/safety_zone/polygon/index.html | 102 + .../polygon/polygon.cpp.func-sort-c.html | 112 + .../safety_zone/polygon/polygon.cpp.func.html | 112 + .../polygon/polygon.cpp.gcov.frameset.html | 19 + .../safety_zone/polygon/polygon.cpp.gcov.html | 290 + .../polygon/polygon.cpp.gcov.overview.html | 72 + .../safety_zone/polygon/polygon.cpp.gcov.png | Bin 0 -> 1057 bytes .../safety_zone.cpp.func-sort-c.html | 104 + .../src/safety_zone/safety_zone.cpp.func.html | 104 + .../safety_zone.cpp.gcov.frameset.html | 19 + .../src/safety_zone/safety_zone.cpp.gcov.html | 160 + .../safety_zone.cpp.gcov.overview.html | 39 + .../src/safety_zone/safety_zone.cpp.gcov.png | Bin 0 -> 380 bytes .../src/scope_timer/index-detail-sort-f.html | 110 + .../src/scope_timer/index-detail-sort-l.html | 110 + mrs_lib/src/scope_timer/index-detail.html | 110 + mrs_lib/src/scope_timer/index-sort-f.html | 102 + mrs_lib/src/scope_timer/index-sort-l.html | 102 + mrs_lib/src/scope_timer/index.html | 102 + .../scope_timer.cpp.func-sort-c.html | 120 + .../src/scope_timer/scope_timer.cpp.func.html | 120 + .../scope_timer.cpp.gcov.frameset.html | 19 + .../src/scope_timer/scope_timer.cpp.gcov.html | 345 + .../scope_timer.cpp.gcov.overview.html | 86 + .../src/scope_timer/scope_timer.cpp.gcov.png | Bin 0 -> 1280 bytes .../timeout_manager/index-detail-sort-f.html | 110 + .../timeout_manager/index-detail-sort-l.html | 110 + mrs_lib/src/timeout_manager/index-detail.html | 110 + mrs_lib/src/timeout_manager/index-sort-f.html | 102 + mrs_lib/src/timeout_manager/index-sort-l.html | 102 + mrs_lib/src/timeout_manager/index.html | 102 + .../timeout_manager.cpp.func-sort-c.html | 124 + .../timeout_manager.cpp.func.html | 124 + .../timeout_manager.cpp.gcov.frameset.html | 19 + .../timeout_manager.cpp.gcov.html | 186 + .../timeout_manager.cpp.gcov.overview.html | 46 + .../timeout_manager.cpp.gcov.png | Bin 0 -> 512 bytes mrs_lib/src/timer/index-detail-sort-f.html | 110 + mrs_lib/src/timer/index-detail-sort-l.html | 110 + mrs_lib/src/timer/index-detail.html | 110 + mrs_lib/src/timer/index-sort-f.html | 102 + mrs_lib/src/timer/index-sort-l.html | 102 + mrs_lib/src/timer/index.html | 102 + mrs_lib/src/timer/timer.cpp.func-sort-c.html | 152 + mrs_lib/src/timer/timer.cpp.func.html | 152 + .../src/timer/timer.cpp.gcov.frameset.html | 19 + mrs_lib/src/timer/timer.cpp.gcov.html | 364 + .../src/timer/timer.cpp.gcov.overview.html | 90 + mrs_lib/src/timer/timer.cpp.gcov.png | Bin 0 -> 1150 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../transform_broadcaster/index-detail.html | 110 + .../transform_broadcaster/index-sort-f.html | 102 + .../transform_broadcaster/index-sort-l.html | 102 + mrs_lib/src/transform_broadcaster/index.html | 102 + ...transform_broadcaster.cpp.func-sort-c.html | 92 + .../transform_broadcaster.cpp.func.html | 92 + ...ansform_broadcaster.cpp.gcov.frameset.html | 19 + .../transform_broadcaster.cpp.gcov.html | 140 + ...ansform_broadcaster.cpp.gcov.overview.html | 34 + .../transform_broadcaster.cpp.gcov.png | Bin 0 -> 425 bytes .../src/transformer/index-detail-sort-f.html | 110 + .../src/transformer/index-detail-sort-l.html | 110 + mrs_lib/src/transformer/index-detail.html | 110 + mrs_lib/src/transformer/index-sort-f.html | 102 + mrs_lib/src/transformer/index-sort-l.html | 102 + mrs_lib/src/transformer/index.html | 102 + .../transformer.cpp.func-sort-c.html | 184 + .../src/transformer/transformer.cpp.func.html | 184 + .../transformer.cpp.gcov.frameset.html | 19 + .../src/transformer/transformer.cpp.gcov.html | 671 ++ .../transformer.cpp.gcov.overview.html | 167 + .../src/transformer/transformer.cpp.gcov.png | Bin 0 -> 1995 bytes mrs_lib/src/utils/index-detail-sort-f.html | 110 + mrs_lib/src/utils/index-detail-sort-l.html | 110 + mrs_lib/src/utils/index-detail.html | 110 + mrs_lib/src/utils/index-sort-f.html | 102 + mrs_lib/src/utils/index-sort-l.html | 102 + mrs_lib/src/utils/index.html | 102 + mrs_lib/src/utils/utils.cpp.func-sort-c.html | 88 + mrs_lib/src/utils/utils.cpp.func.html | 88 + .../src/utils/utils.cpp.gcov.frameset.html | 19 + mrs_lib/src/utils/utils.cpp.gcov.html | 101 + .../src/utils/utils.cpp.gcov.overview.html | 25 + mrs_lib/src/utils/utils.cpp.gcov.png | Bin 0 -> 194 bytes .../src/automatic_start.cpp.func-sort-c.html | 164 + .../src/automatic_start.cpp.func.html | 164 + .../automatic_start.cpp.gcov.frameset.html | 19 + .../src/automatic_start.cpp.gcov.html | 1049 ++ .../automatic_start.cpp.gcov.overview.html | 262 + .../src/automatic_start.cpp.gcov.png | Bin 0 -> 3128 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + mrs_uav_autostart/src/index-detail.html | 110 + mrs_uav_autostart/src/index-sort-f.html | 102 + mrs_uav_autostart/src/index-sort-l.html | 102 + mrs_uav_autostart/src/index.html | 102 + .../include/common.h.func-sort-c.html | 116 + .../include/common.h.func.html | 116 + .../include/common.h.gcov.frameset.html | 19 + .../include/common.h.gcov.html | 189 + .../include/common.h.gcov.overview.html | 47 + mrs_uav_controllers/include/common.h.gcov.png | Bin 0 -> 436 bytes .../include/index-detail-sort-f.html | 128 + .../include/index-detail-sort-l.html | 128 + mrs_uav_controllers/include/index-detail.html | 128 + mrs_uav_controllers/include/index-sort-f.html | 112 + mrs_uav_controllers/include/index-sort-l.html | 112 + mrs_uav_controllers/include/index.html | 112 + .../include/pid.hpp.func-sort-c.html | 100 + mrs_uav_controllers/include/pid.hpp.func.html | 100 + .../include/pid.hpp.gcov.frameset.html | 19 + mrs_uav_controllers/include/pid.hpp.gcov.html | 184 + .../include/pid.hpp.gcov.overview.html | 45 + mrs_uav_controllers/include/pid.hpp.gcov.png | Bin 0 -> 519 bytes .../src/common.cpp.func-sort-c.html | 116 + mrs_uav_controllers/src/common.cpp.func.html | 116 + .../src/common.cpp.gcov.frameset.html | 19 + mrs_uav_controllers/src/common.cpp.gcov.html | 471 + .../src/common.cpp.gcov.overview.html | 117 + mrs_uav_controllers/src/common.cpp.gcov.png | Bin 0 -> 1368 bytes .../failsafe_controller.cpp.func-sort-c.html | 124 + .../src/failsafe_controller.cpp.func.html | 124 + ...failsafe_controller.cpp.gcov.frameset.html | 19 + .../src/failsafe_controller.cpp.gcov.html | 635 ++ ...failsafe_controller.cpp.gcov.overview.html | 158 + .../src/failsafe_controller.cpp.gcov.png | Bin 0 -> 1814 bytes .../src/index-detail-sort-f.html | 182 + .../src/index-detail-sort-l.html | 182 + mrs_uav_controllers/src/index-detail.html | 182 + mrs_uav_controllers/src/index-sort-f.html | 142 + mrs_uav_controllers/src/index-sort-l.html | 142 + mrs_uav_controllers/src/index.html | 142 + ...activation_controller.cpp.func-sort-c.html | 124 + ...midair_activation_controller.cpp.func.html | 124 + ...tivation_controller.cpp.gcov.frameset.html | 19 + ...midair_activation_controller.cpp.gcov.html | 518 + ...tivation_controller.cpp.gcov.overview.html | 129 + .../midair_activation_controller.cpp.gcov.png | Bin 0 -> 1228 bytes .../src/mpc_controller.cpp.func-sort-c.html | 152 + .../src/mpc_controller.cpp.func.html | 152 + .../src/mpc_controller.cpp.gcov.frameset.html | 19 + .../src/mpc_controller.cpp.gcov.html | 2217 ++++ .../src/mpc_controller.cpp.gcov.overview.html | 554 + .../src/mpc_controller.cpp.gcov.png | Bin 0 -> 6670 bytes .../src/se3_controller.cpp.func-sort-c.html | 148 + .../src/se3_controller.cpp.func.html | 148 + .../src/se3_controller.cpp.gcov.frameset.html | 19 + .../src/se3_controller.cpp.gcov.html | 1930 ++++ .../src/se3_controller.cpp.gcov.overview.html | 482 + .../src/se3_controller.cpp.gcov.png | Bin 0 -> 5859 bytes .../control_manager/index-detail-sort-f.html | 110 + .../control_manager/index-detail-sort-l.html | 110 + .../include/control_manager/index-detail.html | 110 + .../include/control_manager/index-sort-f.html | 102 + .../include/control_manager/index-sort-l.html | 102 + .../include/control_manager/index.html | 102 + .../output_publisher.h.func-sort-c.html | 120 + .../output_publisher.h.func.html | 120 + .../output_publisher.h.gcov.frameset.html | 19 + .../output_publisher.h.gcov.html | 146 + .../output_publisher.h.gcov.overview.html | 36 + .../output_publisher.h.gcov.png | Bin 0 -> 332 bytes .../agl_estimator.h.func-sort-c.html | 92 + .../agl_estimator.h.func.html | 92 + .../agl_estimator.h.gcov.frameset.html | 19 + .../agl_estimator.h.gcov.html | 157 + .../agl_estimator.h.gcov.overview.html | 39 + .../mrs_uav_managers/agl_estimator.h.gcov.png | Bin 0 -> 397 bytes .../control_manager/common.h.func-sort-c.html | 188 + .../control_manager/common.h.func.html | 188 + .../common.h.gcov.frameset.html | 19 + .../control_manager/common.h.gcov.html | 380 + .../common.h.gcov.overview.html | 94 + .../control_manager/common.h.gcov.png | Bin 0 -> 947 bytes .../control_manager/index-detail-sort-f.html | 110 + .../control_manager/index-detail-sort-l.html | 110 + .../control_manager/index-detail.html | 110 + .../control_manager/index-sort-f.html | 102 + .../control_manager/index-sort-l.html | 102 + .../control_manager/index.html | 102 + .../controller.h.func-sort-c.html | 88 + .../mrs_uav_managers/controller.h.func.html | 88 + .../controller.h.gcov.frameset.html | 19 + .../mrs_uav_managers/controller.h.gcov.html | 232 + .../controller.h.gcov.overview.html | 57 + .../mrs_uav_managers/controller.h.gcov.png | Bin 0 -> 681 bytes .../estimator.h.func-sort-c.html | 92 + .../estimation_manager/estimator.h.func.html | 92 + .../estimator.h.gcov.frameset.html | 19 + .../estimation_manager/estimator.h.gcov.html | 195 + .../estimator.h.gcov.overview.html | 48 + .../estimation_manager/estimator.h.gcov.png | Bin 0 -> 506 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../estimation_manager/index-detail.html | 128 + .../estimation_manager/index-sort-f.html | 112 + .../estimation_manager/index-sort-l.html | 112 + .../estimation_manager/index.html | 112 + .../support.h.func-sort-c.html | 140 + .../estimation_manager/support.h.func.html | 140 + .../support.h.gcov.frameset.html | 19 + .../estimation_manager/support.h.gcov.html | 405 + .../support.h.gcov.overview.html | 101 + .../estimation_manager/support.h.gcov.png | Bin 0 -> 1232 bytes .../mrs_uav_managers/index-detail-sort-f.html | 164 + .../mrs_uav_managers/index-detail-sort-l.html | 164 + .../mrs_uav_managers/index-detail.html | 164 + .../mrs_uav_managers/index-sort-f.html | 132 + .../mrs_uav_managers/index-sort-l.html | 132 + .../include/mrs_uav_managers/index.html | 132 + .../state_estimator.h.func-sort-c.html | 92 + .../state_estimator.h.func.html | 92 + .../state_estimator.h.gcov.frameset.html | 19 + .../state_estimator.h.gcov.html | 169 + .../state_estimator.h.gcov.overview.html | 42 + .../state_estimator.h.gcov.png | Bin 0 -> 435 bytes .../tracker.h.func-sort-c.html | 88 + .../mrs_uav_managers/tracker.h.func.html | 88 + .../tracker.h.gcov.frameset.html | 19 + .../mrs_uav_managers/tracker.h.gcov.html | 296 + .../tracker.h.gcov.overview.html | 73 + .../mrs_uav_managers/tracker.h.gcov.png | Bin 0 -> 751 bytes .../index-detail-sort-f.html | 120 + .../index-detail-sort-l.html | 120 + .../transform_manager/index-detail.html | 120 + .../transform_manager/index-sort-f.html | 112 + .../transform_manager/index-sort-l.html | 112 + .../include/transform_manager/index.html | 112 + .../tf_mapping_origin.h.func-sort-c.html | 100 + .../tf_mapping_origin.h.func.html | 100 + .../tf_mapping_origin.h.gcov.frameset.html | 19 + .../tf_mapping_origin.h.gcov.html | 474 + .../tf_mapping_origin.h.gcov.overview.html | 118 + .../tf_mapping_origin.h.gcov.png | Bin 0 -> 1536 bytes .../tf_source.h.func-sort-c.html | 136 + .../transform_manager/tf_source.h.func.html | 136 + .../tf_source.h.gcov.frameset.html | 19 + .../transform_manager/tf_source.h.gcov.html | 704 ++ .../tf_source.h.gcov.overview.html | 175 + .../transform_manager/tf_source.h.gcov.png | Bin 0 -> 2210 bytes .../constraint_manager.cpp.func-sort-c.html | 112 + .../src/constraint_manager.cpp.func.html | 112 + .../constraint_manager.cpp.gcov.frameset.html | 19 + .../src/constraint_manager.cpp.gcov.html | 716 ++ .../constraint_manager.cpp.gcov.overview.html | 178 + .../src/constraint_manager.cpp.gcov.png | Bin 0 -> 2190 bytes .../common/common.cpp.func-sort-c.html | 204 + .../common/common.cpp.func.html | 204 + .../common/common.cpp.gcov.frameset.html | 19 + .../common/common.cpp.gcov.html | 1312 +++ .../common/common.cpp.gcov.overview.html | 327 + .../common/common.cpp.gcov.png | Bin 0 -> 2462 bytes .../common/index-detail-sort-f.html | 110 + .../common/index-detail-sort-l.html | 110 + .../control_manager/common/index-detail.html | 110 + .../control_manager/common/index-sort-f.html | 102 + .../control_manager/common/index-sort-l.html | 102 + .../src/control_manager/common/index.html | 102 + .../control_manager.cpp.func-sort-c.html | 524 + .../control_manager.cpp.func.html | 524 + .../control_manager.cpp.gcov.frameset.html | 19 + .../control_manager.cpp.gcov.html | 8871 +++++++++++++++++ .../control_manager.cpp.gcov.overview.html | 2217 ++++ .../control_manager.cpp.gcov.png | Bin 0 -> 26085 bytes .../control_manager/index-detail-sort-f.html | 128 + .../control_manager/index-detail-sort-l.html | 128 + .../src/control_manager/index-detail.html | 128 + .../src/control_manager/index-sort-f.html | 112 + .../src/control_manager/index-sort-l.html | 112 + .../src/control_manager/index.html | 112 + .../output_publisher.cpp.func-sort-c.html | 128 + .../output_publisher.cpp.func.html | 128 + .../output_publisher.cpp.gcov.frameset.html | 19 + .../output_publisher.cpp.gcov.html | 154 + .../output_publisher.cpp.gcov.overview.html | 38 + .../output_publisher.cpp.gcov.png | Bin 0 -> 307 bytes .../estimation_manager.cpp.func-sort-c.html | 176 + .../estimation_manager.cpp.func.html | 176 + .../estimation_manager.cpp.gcov.frameset.html | 19 + .../estimation_manager.cpp.gcov.html | 1338 +++ .../estimation_manager.cpp.gcov.overview.html | 334 + .../estimation_manager.cpp.gcov.png | Bin 0 -> 4359 bytes .../estimator.cpp.func-sort-c.html | 172 + .../estimator.cpp.func.html | 172 + .../estimator.cpp.gcov.frameset.html | 19 + .../estimator.cpp.gcov.html | 295 + .../estimator.cpp.gcov.overview.html | 73 + .../estimation_manager/estimator.cpp.gcov.png | Bin 0 -> 828 bytes .../agl_estimator.cpp.func-sort-c.html | 92 + .../estimators/agl_estimator.cpp.func.html | 92 + .../agl_estimator.cpp.gcov.frameset.html | 19 + .../estimators/agl_estimator.cpp.gcov.html | 182 + .../agl_estimator.cpp.gcov.overview.html | 45 + .../estimators/agl_estimator.cpp.gcov.png | Bin 0 -> 454 bytes .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimation_manager/estimators/index.html | 112 + .../state_estimator.cpp.func-sort-c.html | 120 + .../estimators/state_estimator.cpp.func.html | 120 + .../state_estimator.cpp.gcov.frameset.html | 19 + .../estimators/state_estimator.cpp.gcov.html | 263 + .../state_estimator.cpp.gcov.overview.html | 65 + .../estimators/state_estimator.cpp.gcov.png | Bin 0 -> 753 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../src/estimation_manager/index-detail.html | 128 + .../src/estimation_manager/index-sort-f.html | 112 + .../src/estimation_manager/index-sort-l.html | 112 + .../src/estimation_manager/index.html | 112 + .../src/gain_manager.cpp.func-sort-c.html | 108 + .../src/gain_manager.cpp.func.html | 108 + .../src/gain_manager.cpp.gcov.frameset.html | 19 + .../src/gain_manager.cpp.gcov.html | 730 ++ .../src/gain_manager.cpp.gcov.overview.html | 182 + .../src/gain_manager.cpp.gcov.png | Bin 0 -> 2153 bytes mrs_uav_managers/src/index-detail-sort-f.html | 164 + mrs_uav_managers/src/index-detail-sort-l.html | 164 + mrs_uav_managers/src/index-detail.html | 164 + mrs_uav_managers/src/index-sort-f.html | 132 + mrs_uav_managers/src/index-sort-l.html | 132 + mrs_uav_managers/src/index.html | 132 + .../src/null_tracker.cpp.func-sort-c.html | 160 + .../src/null_tracker.cpp.func.html | 160 + .../src/null_tracker.cpp.gcov.frameset.html | 19 + .../src/null_tracker.cpp.gcov.html | 332 + .../src/null_tracker.cpp.gcov.overview.html | 82 + .../src/null_tracker.cpp.gcov.png | Bin 0 -> 809 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../src/transform_manager/index-detail.html | 110 + .../src/transform_manager/index-sort-f.html | 102 + .../src/transform_manager/index-sort-l.html | 102 + .../src/transform_manager/index.html | 102 + .../transform_manager.cpp.func-sort-c.html | 136 + .../transform_manager.cpp.func.html | 136 + .../transform_manager.cpp.gcov.frameset.html | 19 + .../transform_manager.cpp.gcov.html | 1038 ++ .../transform_manager.cpp.gcov.overview.html | 259 + .../transform_manager.cpp.gcov.png | Bin 0 -> 3383 bytes .../src/uav_manager.cpp.func-sort-c.html | 232 + .../src/uav_manager.cpp.func.html | 232 + .../src/uav_manager.cpp.gcov.frameset.html | 19 + .../src/uav_manager.cpp.gcov.html | 2765 +++++ .../src/uav_manager.cpp.gcov.overview.html | 691 ++ mrs_uav_managers/src/uav_manager.cpp.gcov.png | Bin 0 -> 7292 bytes .../agl/garmin_agl.h.func-sort-c.html | 92 + .../estimators/agl/garmin_agl.h.func.html | 92 + .../agl/garmin_agl.h.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.h.gcov.html | 159 + .../agl/garmin_agl.h.gcov.overview.html | 39 + .../estimators/agl/garmin_agl.h.gcov.png | Bin 0 -> 380 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../estimators/agl/index-detail.html | 110 + .../estimators/agl/index-sort-f.html | 102 + .../estimators/agl/index-sort-l.html | 102 + .../estimators/agl/index.html | 102 + .../altitude/alt_generic.h.func-sort-c.html | 92 + .../altitude/alt_generic.h.func.html | 92 + .../altitude/alt_generic.h.gcov.frameset.html | 19 + .../altitude/alt_generic.h.gcov.html | 245 + .../altitude/alt_generic.h.gcov.overview.html | 61 + .../altitude/alt_generic.h.gcov.png | Bin 0 -> 688 bytes .../altitude_estimator.h.func-sort-c.html | 92 + .../altitude/altitude_estimator.h.func.html | 92 + .../altitude_estimator.h.gcov.frameset.html | 19 + .../altitude/altitude_estimator.h.gcov.html | 130 + .../altitude_estimator.h.gcov.overview.html | 32 + .../altitude/altitude_estimator.h.gcov.png | Bin 0 -> 295 bytes .../altitude/index-detail-sort-f.html | 128 + .../altitude/index-detail-sort-l.html | 128 + .../estimators/altitude/index-detail.html | 128 + .../estimators/altitude/index-sort-f.html | 112 + .../estimators/altitude/index-sort-l.html | 112 + .../estimators/altitude/index.html | 112 + .../estimators/correction.h.func-sort-c.html | 364 + .../estimators/correction.h.func.html | 364 + .../correction.h.gcov.frameset.html | 19 + .../estimators/correction.h.gcov.html | 1948 ++++ .../correction.h.gcov.overview.html | 486 + .../estimators/correction.h.gcov.png | Bin 0 -> 5376 bytes .../heading/hdg_generic.h.func-sort-c.html | 92 + .../heading/hdg_generic.h.func.html | 92 + .../heading/hdg_generic.h.gcov.frameset.html | 19 + .../heading/hdg_generic.h.gcov.html | 243 + .../heading/hdg_generic.h.gcov.overview.html | 60 + .../estimators/heading/hdg_generic.h.gcov.png | Bin 0 -> 663 bytes .../hdg_passthrough.h.func-sort-c.html | 92 + .../heading/hdg_passthrough.h.func.html | 92 + .../hdg_passthrough.h.gcov.frameset.html | 19 + .../heading/hdg_passthrough.h.gcov.html | 191 + .../hdg_passthrough.h.gcov.overview.html | 47 + .../heading/hdg_passthrough.h.gcov.png | Bin 0 -> 506 bytes .../heading_estimator.h.func-sort-c.html | 84 + .../heading/heading_estimator.h.func.html | 84 + .../heading_estimator.h.gcov.frameset.html | 19 + .../heading/heading_estimator.h.gcov.html | 124 + .../heading_estimator.h.gcov.overview.html | 30 + .../heading/heading_estimator.h.gcov.png | Bin 0 -> 293 bytes .../heading/index-detail-sort-f.html | 138 + .../heading/index-detail-sort-l.html | 138 + .../estimators/heading/index-detail.html | 138 + .../estimators/heading/index-sort-f.html | 122 + .../estimators/heading/index-sort-l.html | 122 + .../estimators/heading/index.html | 122 + .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimators/index.html | 112 + .../lateral/index-detail-sort-f.html | 128 + .../lateral/index-detail-sort-l.html | 128 + .../estimators/lateral/index-detail.html | 128 + .../estimators/lateral/index-sort-f.html | 112 + .../estimators/lateral/index-sort-l.html | 112 + .../estimators/lateral/index.html | 112 + .../lateral/lat_generic.h.func-sort-c.html | 92 + .../lateral/lat_generic.h.func.html | 92 + .../lateral/lat_generic.h.gcov.frameset.html | 19 + .../lateral/lat_generic.h.gcov.html | 250 + .../lateral/lat_generic.h.gcov.overview.html | 62 + .../estimators/lateral/lat_generic.h.gcov.png | Bin 0 -> 708 bytes .../lateral_estimator.h.func-sort-c.html | 92 + .../lateral/lateral_estimator.h.func.html | 92 + .../lateral_estimator.h.gcov.frameset.html | 19 + .../lateral/lateral_estimator.h.gcov.html | 122 + .../lateral_estimator.h.gcov.overview.html | 30 + .../lateral/lateral_estimator.h.gcov.png | Bin 0 -> 284 bytes .../partial_estimator.h.func-sort-c.html | 176 + .../estimators/partial_estimator.h.func.html | 176 + .../partial_estimator.h.gcov.frameset.html | 19 + .../estimators/partial_estimator.h.gcov.html | 264 + .../partial_estimator.h.gcov.overview.html | 65 + .../estimators/partial_estimator.h.gcov.png | Bin 0 -> 820 bytes .../estimators/state/index-detail-sort-f.html | 128 + .../estimators/state/index-detail-sort-l.html | 128 + .../estimators/state/index-detail.html | 128 + .../estimators/state/index-sort-f.html | 112 + .../estimators/state/index-sort-l.html | 112 + .../estimators/state/index.html | 112 + .../state/passthrough.h.func-sort-c.html | 92 + .../estimators/state/passthrough.h.func.html | 92 + .../state/passthrough.h.gcov.frameset.html | 19 + .../estimators/state/passthrough.h.gcov.html | 173 + .../state/passthrough.h.gcov.overview.html | 43 + .../estimators/state/passthrough.h.gcov.png | Bin 0 -> 431 bytes .../state/state_generic.h.func-sort-c.html | 92 + .../state/state_generic.h.func.html | 92 + .../state/state_generic.h.gcov.frameset.html | 19 + .../state/state_generic.h.gcov.html | 183 + .../state/state_generic.h.gcov.overview.html | 45 + .../estimators/state/state_generic.h.gcov.png | Bin 0 -> 438 bytes .../processors/index-detail-sort-f.html | 182 + .../processors/index-detail-sort-l.html | 182 + .../processors/index-detail.html | 182 + .../processors/index-sort-f.html | 142 + .../processors/index-sort-l.html | 142 + .../processors/index.html | 142 + .../proc_excessive_tilt.h.func-sort-c.html | 104 + .../proc_excessive_tilt.h.func.html | 104 + .../proc_excessive_tilt.h.gcov.frameset.html | 19 + .../proc_excessive_tilt.h.gcov.html | 206 + .../proc_excessive_tilt.h.gcov.overview.html | 51 + .../processors/proc_excessive_tilt.h.gcov.png | Bin 0 -> 656 bytes .../proc_median_filter.h.func-sort-c.html | 104 + .../processors/proc_median_filter.h.func.html | 104 + .../proc_median_filter.h.gcov.frameset.html | 19 + .../processors/proc_median_filter.h.gcov.html | 192 + .../proc_median_filter.h.gcov.overview.html | 47 + .../processors/proc_median_filter.h.gcov.png | Bin 0 -> 635 bytes .../proc_saturate.h.func-sort-c.html | 104 + .../processors/proc_saturate.h.func.html | 104 + .../proc_saturate.h.gcov.frameset.html | 19 + .../processors/proc_saturate.h.gcov.html | 202 + .../proc_saturate.h.gcov.overview.html | 50 + .../processors/proc_saturate.h.gcov.png | Bin 0 -> 711 bytes .../proc_tf_to_world.h.func-sort-c.html | 112 + .../processors/proc_tf_to_world.h.func.html | 112 + .../proc_tf_to_world.h.gcov.frameset.html | 19 + .../processors/proc_tf_to_world.h.gcov.html | 241 + .../proc_tf_to_world.h.gcov.overview.html | 60 + .../processors/proc_tf_to_world.h.gcov.png | Bin 0 -> 777 bytes .../processors/processor.h.func-sort-c.html | 112 + .../processors/processor.h.func.html | 112 + .../processors/processor.h.gcov.frameset.html | 19 + .../processors/processor.h.gcov.html | 156 + .../processors/processor.h.gcov.overview.html | 38 + .../processors/processor.h.gcov.png | Bin 0 -> 436 bytes .../agl/garmin_agl.cpp.func-sort-c.html | 120 + .../estimators/agl/garmin_agl.cpp.func.html | 120 + .../agl/garmin_agl.cpp.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.cpp.gcov.html | 318 + .../agl/garmin_agl.cpp.gcov.overview.html | 79 + .../estimators/agl/garmin_agl.cpp.gcov.png | Bin 0 -> 1043 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../src/estimators/agl/index-detail.html | 110 + .../src/estimators/agl/index-sort-f.html | 102 + .../src/estimators/agl/index-sort-l.html | 102 + .../src/estimators/agl/index.html | 102 + .../altitude/alt_generic.cpp.func-sort-c.html | 212 + .../altitude/alt_generic.cpp.func.html | 212 + .../alt_generic.cpp.gcov.frameset.html | 19 + .../altitude/alt_generic.cpp.gcov.html | 839 ++ .../alt_generic.cpp.gcov.overview.html | 209 + .../altitude/alt_generic.cpp.gcov.png | Bin 0 -> 2919 bytes .../altitude/index-detail-sort-f.html | 110 + .../altitude/index-detail-sort-l.html | 110 + .../src/estimators/altitude/index-detail.html | 110 + .../src/estimators/altitude/index-sort-f.html | 102 + .../src/estimators/altitude/index-sort-l.html | 102 + .../src/estimators/altitude/index.html | 102 + .../heading/hdg_generic.cpp.func-sort-c.html | 216 + .../heading/hdg_generic.cpp.func.html | 216 + .../hdg_generic.cpp.gcov.frameset.html | 19 + .../heading/hdg_generic.cpp.gcov.html | 795 ++ .../hdg_generic.cpp.gcov.overview.html | 198 + .../heading/hdg_generic.cpp.gcov.png | Bin 0 -> 2310 bytes .../hdg_passthrough.cpp.func-sort-c.html | 172 + .../heading/hdg_passthrough.cpp.func.html | 172 + .../hdg_passthrough.cpp.gcov.frameset.html | 19 + .../heading/hdg_passthrough.cpp.gcov.html | 391 + .../hdg_passthrough.cpp.gcov.overview.html | 97 + .../heading/hdg_passthrough.cpp.gcov.png | Bin 0 -> 1198 bytes .../heading/index-detail-sort-f.html | 120 + .../heading/index-detail-sort-l.html | 120 + .../src/estimators/heading/index-detail.html | 120 + .../src/estimators/heading/index-sort-f.html | 112 + .../src/estimators/heading/index-sort-l.html | 112 + .../src/estimators/heading/index.html | 112 + .../lateral/index-detail-sort-f.html | 110 + .../lateral/index-detail-sort-l.html | 110 + .../src/estimators/lateral/index-detail.html | 110 + .../src/estimators/lateral/index-sort-f.html | 102 + .../src/estimators/lateral/index-sort-l.html | 102 + .../src/estimators/lateral/index.html | 102 + .../lateral/lat_generic.cpp.func-sort-c.html | 212 + .../lateral/lat_generic.cpp.func.html | 212 + .../lat_generic.cpp.gcov.frameset.html | 19 + .../lateral/lat_generic.cpp.gcov.html | 887 ++ .../lat_generic.cpp.gcov.overview.html | 221 + .../lateral/lat_generic.cpp.gcov.png | Bin 0 -> 3149 bytes .../state/gps_baro.cpp.func-sort-c.html | 96 + .../estimators/state/gps_baro.cpp.func.html | 96 + .../state/gps_baro.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_baro.cpp.gcov.html | 109 + .../state/gps_baro.cpp.gcov.overview.html | 27 + .../estimators/state/gps_baro.cpp.gcov.png | Bin 0 -> 231 bytes .../state/gps_garmin.cpp.func-sort-c.html | 96 + .../estimators/state/gps_garmin.cpp.func.html | 96 + .../state/gps_garmin.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_garmin.cpp.gcov.html | 109 + .../state/gps_garmin.cpp.gcov.overview.html | 27 + .../estimators/state/gps_garmin.cpp.gcov.png | Bin 0 -> 231 bytes .../estimators/state/index-detail-sort-f.html | 200 + .../estimators/state/index-detail-sort-l.html | 200 + .../src/estimators/state/index-detail.html | 200 + .../src/estimators/state/index-sort-f.html | 152 + .../src/estimators/state/index-sort-l.html | 152 + .../src/estimators/state/index.html | 152 + .../state/passthrough.cpp.func-sort-c.html | 116 + .../state/passthrough.cpp.func.html | 116 + .../state/passthrough.cpp.gcov.frameset.html | 19 + .../state/passthrough.cpp.gcov.html | 343 + .../state/passthrough.cpp.gcov.overview.html | 85 + .../estimators/state/passthrough.cpp.gcov.png | Bin 0 -> 1099 bytes .../estimators/state/rtk.cpp.func-sort-c.html | 96 + .../src/estimators/state/rtk.cpp.func.html | 96 + .../state/rtk.cpp.gcov.frameset.html | 19 + .../src/estimators/state/rtk.cpp.gcov.html | 109 + .../state/rtk.cpp.gcov.overview.html | 27 + .../src/estimators/state/rtk.cpp.gcov.png | Bin 0 -> 229 bytes .../state/rtk_garmin.cpp.func-sort-c.html | 96 + .../estimators/state/rtk_garmin.cpp.func.html | 96 + .../state/rtk_garmin.cpp.gcov.frameset.html | 19 + .../estimators/state/rtk_garmin.cpp.gcov.html | 109 + .../state/rtk_garmin.cpp.gcov.overview.html | 27 + .../estimators/state/rtk_garmin.cpp.gcov.png | Bin 0 -> 231 bytes .../state/state_generic.cpp.func-sort-c.html | 128 + .../state/state_generic.cpp.func.html | 128 + .../state_generic.cpp.gcov.frameset.html | 19 + .../state/state_generic.cpp.gcov.html | 633 ++ .../state_generic.cpp.gcov.overview.html | 158 + .../state/state_generic.cpp.gcov.png | Bin 0 -> 1968 bytes .../src/joy_tracker/index-detail-sort-f.html | 110 + .../src/joy_tracker/index-detail-sort-l.html | 110 + .../src/joy_tracker/index-detail.html | 110 + .../src/joy_tracker/index-sort-f.html | 102 + .../src/joy_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/joy_tracker/index.html | 102 + .../joy_tracker.cpp.func-sort-c.html | 152 + .../src/joy_tracker/joy_tracker.cpp.func.html | 152 + .../joy_tracker.cpp.gcov.frameset.html | 19 + .../src/joy_tracker/joy_tracker.cpp.gcov.html | 588 ++ .../joy_tracker.cpp.gcov.overview.html | 146 + .../src/joy_tracker/joy_tracker.cpp.gcov.png | Bin 0 -> 1740 bytes .../landoff_tracker/index-detail-sort-f.html | 110 + .../landoff_tracker/index-detail-sort-l.html | 110 + .../src/landoff_tracker/index-detail.html | 110 + .../src/landoff_tracker/index-sort-f.html | 102 + .../src/landoff_tracker/index-sort-l.html | 102 + .../src/landoff_tracker/index.html | 102 + .../landoff_tracker.cpp.func-sort-c.html | 204 + .../landoff_tracker.cpp.func.html | 204 + .../landoff_tracker.cpp.gcov.frameset.html | 19 + .../landoff_tracker.cpp.gcov.html | 1636 +++ .../landoff_tracker.cpp.gcov.overview.html | 408 + .../landoff_tracker.cpp.gcov.png | Bin 0 -> 4772 bytes .../src/line_tracker/index-detail-sort-f.html | 110 + .../src/line_tracker/index-detail-sort-l.html | 110 + .../src/line_tracker/index-detail.html | 110 + .../src/line_tracker/index-sort-f.html | 102 + .../src/line_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/line_tracker/index.html | 102 + .../line_tracker.cpp.func-sort-c.html | 200 + .../line_tracker/line_tracker.cpp.func.html | 200 + .../line_tracker.cpp.gcov.frameset.html | 19 + .../line_tracker/line_tracker.cpp.gcov.html | 1359 +++ .../line_tracker.cpp.gcov.overview.html | 339 + .../line_tracker/line_tracker.cpp.gcov.png | Bin 0 -> 3780 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../index-detail.html | 110 + .../index-sort-f.html | 102 + .../index-sort-l.html | 102 + .../src/midair_activation_tracker/index.html | 102 + ...ir_activation_tracker.cpp.func-sort-c.html | 152 + .../midair_activation_tracker.cpp.func.html | 152 + ..._activation_tracker.cpp.gcov.frameset.html | 19 + .../midair_activation_tracker.cpp.gcov.html | 426 + ..._activation_tracker.cpp.gcov.overview.html | 106 + .../midair_activation_tracker.cpp.gcov.png | Bin 0 -> 1153 bytes .../src/mpc_tracker/index-detail-sort-f.html | 110 + .../src/mpc_tracker/index-detail-sort-l.html | 110 + .../src/mpc_tracker/index-detail.html | 110 + .../src/mpc_tracker/index-sort-f.html | 102 + .../src/mpc_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/mpc_tracker/index.html | 102 + .../mpc_tracker.cpp.func-sort-c.html | 280 + .../src/mpc_tracker/mpc_tracker.cpp.func.html | 280 + .../mpc_tracker.cpp.gcov.frameset.html | 19 + .../src/mpc_tracker/mpc_tracker.cpp.gcov.html | 3945 ++++++++ .../mpc_tracker.cpp.gcov.overview.html | 986 ++ .../src/mpc_tracker/mpc_tracker.cpp.gcov.png | Bin 0 -> 12420 bytes .../speed_tracker/index-detail-sort-f.html | 110 + .../speed_tracker/index-detail-sort-l.html | 110 + .../src/speed_tracker/index-detail.html | 110 + .../src/speed_tracker/index-sort-f.html | 102 + .../src/speed_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/speed_tracker/index.html | 102 + .../speed_tracker.cpp.func-sort-c.html | 156 + .../speed_tracker/speed_tracker.cpp.func.html | 156 + .../speed_tracker.cpp.gcov.frameset.html | 19 + .../speed_tracker/speed_tracker.cpp.gcov.html | 1178 +++ .../speed_tracker.cpp.gcov.overview.html | 294 + .../speed_tracker/speed_tracker.cpp.gcov.png | Bin 0 -> 3011 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + .../src/index-detail.html | 110 + .../src/index-sort-f.html | 102 + .../src/index-sort-l.html | 102 + mrs_uav_trajectory_generation/src/index.html | 102 + ...trajectory_generation.cpp.func-sort-c.html | 168 + .../mrs_trajectory_generation.cpp.func.html | 168 + ...ajectory_generation.cpp.gcov.frameset.html | 19 + .../mrs_trajectory_generation.cpp.gcov.html | 2458 +++++ ...ajectory_generation.cpp.gcov.overview.html | 614 ++ .../mrs_trajectory_generation.cpp.gcov.png | Bin 0 -> 7279 bytes ruby.png | Bin 0 -> 141 bytes snow.png | Bin 0 -> 141 bytes updown.png | Bin 0 -> 117 bytes 1020 files changed, 179245 insertions(+) create mode 100644 .nojekyll create mode 100644 amber.png create mode 100644 badge.svg create mode 100644 emerald.png create mode 100644 gcov.css create mode 100644 glass.png create mode 100644 index-sort-f.html create mode 100644 index-sort-l.html create mode 100644 index.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/utils.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.func.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.func.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.png create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.png create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index-detail.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/conversions.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.func.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/src/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/src/geometry/index-detail.html create mode 100644 mrs_lib/src/geometry/index-sort-f.html create mode 100644 mrs_lib/src/geometry/index-sort-l.html create mode 100644 mrs_lib/src/geometry/index.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/shapes.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.func.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.png create mode 100644 mrs_lib/src/math/index-detail-sort-f.html create mode 100644 mrs_lib/src/math/index-detail-sort-l.html create mode 100644 mrs_lib/src/math/index-detail.html create mode 100644 mrs_lib/src/math/index-sort-f.html create mode 100644 mrs_lib/src/math/index-sort-l.html create mode 100644 mrs_lib/src/math/index.html create mode 100644 mrs_lib/src/math/math.cpp.func-sort-c.html create mode 100644 mrs_lib/src/math/math.cpp.func.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.overview.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.png create mode 100644 mrs_lib/src/median_filter/index-detail-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-detail-sort-l.html create mode 100644 mrs_lib/src/median_filter/index-detail.html create mode 100644 mrs_lib/src/median_filter/index-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-sort-l.html create mode 100644 mrs_lib/src/median_filter/index.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.png create mode 100644 mrs_lib/src/param_loader/index-detail-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-detail-sort-l.html create mode 100644 mrs_lib/src/param_loader/index-detail.html create mode 100644 mrs_lib/src/param_loader/index-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-sort-l.html create mode 100644 mrs_lib/src/param_loader/index.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.png create mode 100644 mrs_lib/src/profiler/index-detail-sort-f.html create mode 100644 mrs_lib/src/profiler/index-detail-sort-l.html create mode 100644 mrs_lib/src/profiler/index-detail.html create mode 100644 mrs_lib/src/profiler/index-sort-f.html create mode 100644 mrs_lib/src/profiler/index-sort-l.html create mode 100644 mrs_lib/src/profiler/index.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func-sort-c.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.overview.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index-detail.html create mode 100644 mrs_lib/src/safety_zone/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index-detail.html create mode 100644 mrs_lib/src/scope_timer/index-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index-detail.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png create mode 100644 mrs_lib/src/timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/timer/index-detail.html create mode 100644 mrs_lib/src/timer/index-sort-f.html create mode 100644 mrs_lib/src/timer/index-sort-l.html create mode 100644 mrs_lib/src/timer/index.html create mode 100644 mrs_lib/src/timer/timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timer/timer.cpp.func.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.png create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png create mode 100644 mrs_lib/src/transformer/index-detail-sort-f.html create mode 100644 mrs_lib/src/transformer/index-detail-sort-l.html create mode 100644 mrs_lib/src/transformer/index-detail.html create mode 100644 mrs_lib/src/transformer/index-sort-f.html create mode 100644 mrs_lib/src/transformer/index-sort-l.html create mode 100644 mrs_lib/src/transformer/index.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.png create mode 100644 mrs_lib/src/utils/index-detail-sort-f.html create mode 100644 mrs_lib/src/utils/index-detail-sort-l.html create mode 100644 mrs_lib/src/utils/index-detail.html create mode 100644 mrs_lib/src/utils/index-sort-f.html create mode 100644 mrs_lib/src/utils/index-sort-l.html create mode 100644 mrs_lib/src/utils/index.html create mode 100644 mrs_lib/src/utils/utils.cpp.func-sort-c.html create mode 100644 mrs_lib/src/utils/utils.cpp.func.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.overview.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func-sort-c.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.frameset.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/index-detail-sort-f.html create mode 100644 mrs_uav_autostart/src/index-detail-sort-l.html create mode 100644 mrs_uav_autostart/src/index-detail.html create mode 100644 mrs_uav_autostart/src/index-sort-f.html create mode 100644 mrs_uav_autostart/src/index-sort-l.html create mode 100644 mrs_uav_autostart/src/index.html create mode 100644 mrs_uav_controllers/include/common.h.func-sort-c.html create mode 100644 mrs_uav_controllers/include/common.h.func.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.overview.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.png create mode 100644 mrs_uav_controllers/include/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/include/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/include/index-detail.html create mode 100644 mrs_uav_controllers/include/index-sort-f.html create mode 100644 mrs_uav_controllers/include/index-sort-l.html create mode 100644 mrs_uav_controllers/include/index.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func-sort-c.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.overview.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.png create mode 100644 mrs_uav_controllers/src/common.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/common.cpp.func.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/src/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/src/index-detail.html create mode 100644 mrs_uav_controllers/src/index-sort-f.html create mode 100644 mrs_uav_controllers/src/index-sort-l.html create mode 100644 mrs_uav_controllers/src/index.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.png create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/index-detail.html create mode 100644 mrs_uav_managers/src/index-sort-f.html create mode 100644 mrs_uav_managers/src/index-sort-l.html create mode 100644 mrs_uav_managers/src/index.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.png create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/index-detail.html create mode 100644 mrs_uav_trajectory_generation/src/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/index.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.png create mode 100644 ruby.png create mode 100644 snow.png create mode 100644 updown.png diff --git a/.nojekyll b/.nojekyll new file mode 100644 index 0000000000..e69de29bb2 diff --git a/amber.png b/amber.png new file mode 100644 index 0000000000000000000000000000000000000000..2cab170d8359081983a4e343848dfe06bc490f12 GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^G2tW}LqE04T&+ z;1OBOz`!j8!i<;h*8KqrvZOouIx;Y9?C1WI$O`1M1^9%x{(levWGtest coveragetest coverage63.0%63.0% \ No newline at end of file diff --git a/emerald.png b/emerald.png new file mode 100644 index 0000000000000000000000000000000000000000..38ad4f4068b935643d2486f323005fb294a9bd7e GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^Jb!lvI6;R0X`wF(yt=9xVZRt1vCRixIA4P dLn>}1Cji+@42)0J?}79&c)I$ztaD0e0sy@GAL0N2 literal 0 HcmV?d00001 diff --git a/gcov.css b/gcov.css new file mode 100644 index 0000000000..bfd0a83e10 --- /dev/null +++ b/gcov.css @@ -0,0 +1,519 @@ +/* All views: initial background and text color */ +body +{ + color: #000000; + background-color: #FFFFFF; +} + +/* All views: standard link format*/ +a:link +{ + color: #284FA8; + text-decoration: underline; +} + +/* All views: standard link - visited format */ +a:visited +{ + color: #00CB40; + text-decoration: underline; +} + +/* All views: standard link - activated format */ +a:active +{ + color: #FF0040; + text-decoration: underline; +} + +/* All views: main title format */ +td.title +{ + text-align: center; + padding-bottom: 10px; + font-family: sans-serif; + font-size: 20pt; + font-style: italic; + font-weight: bold; +} + +/* All views: header item format */ +td.headerItem +{ + text-align: right; + padding-right: 6px; + font-family: sans-serif; + font-weight: bold; + vertical-align: top; + white-space: nowrap; +} + +/* All views: header item value format */ +td.headerValue +{ + text-align: left; + color: #284FA8; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; +} + +/* All views: header item coverage table heading */ +td.headerCovTableHead +{ + text-align: center; + padding-right: 6px; + padding-left: 6px; + padding-bottom: 0px; + font-family: sans-serif; + font-size: 80%; + white-space: nowrap; +} + +/* All views: header item coverage table entry */ +td.headerCovTableEntry +{ + text-align: right; + color: #284FA8; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #DAE7FE; +} + +/* All views: header item coverage table entry for high coverage rate */ +td.headerCovTableEntryHi +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #A7FC9D; +} + +/* All views: header item coverage table entry for medium coverage rate */ +td.headerCovTableEntryMed +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #FFEA20; +} + +/* All views: header item coverage table entry for ow coverage rate */ +td.headerCovTableEntryLo +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #FF0000; +} + +/* All views: header legend value for legend entry */ +td.headerValueLeg +{ + text-align: left; + color: #000000; + font-family: sans-serif; + font-size: 80%; + white-space: nowrap; + padding-top: 4px; +} + +/* All views: color of horizontal ruler */ +td.ruler +{ + background-color: #6688D4; +} + +/* All views: version string format */ +td.versionInfo +{ + text-align: center; + padding-top: 2px; + font-family: sans-serif; + font-style: italic; +} + +/* Directory view/File view (all)/Test case descriptions: + table headline format */ +td.tableHead +{ + text-align: center; + color: #FFFFFF; + background-color: #6688D4; + font-family: sans-serif; + font-size: 120%; + font-weight: bold; + white-space: nowrap; + padding-left: 4px; + padding-right: 4px; +} + +span.tableHeadSort +{ + padding-right: 4px; +} + +/* Directory view/File view (all): filename entry format */ +td.coverFile +{ + text-align: left; + padding-left: 10px; + padding-right: 20px; + color: #284FA8; + background-color: #DAE7FE; + font-family: monospace; +} + +/* Directory view/File view (all): bar-graph entry format*/ +td.coverBar +{ + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; +} + +/* Directory view/File view (all): bar-graph outline color */ +td.coverBarOutline +{ + background-color: #000000; +} + +/* Directory view/File view (all): percentage entry for files with + high coverage rate */ +td.coverPerHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #A7FC9D; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + high coverage rate */ +td.coverNumHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #A7FC9D; + white-space: nowrap; + font-family: sans-serif; +} + +/* Directory view/File view (all): percentage entry for files with + medium coverage rate */ +td.coverPerMed +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FFEA20; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + medium coverage rate */ +td.coverNumMed +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FFEA20; + white-space: nowrap; + font-family: sans-serif; +} + +/* Directory view/File view (all): percentage entry for files with + low coverage rate */ +td.coverPerLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + low coverage rate */ +td.coverNumLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + white-space: nowrap; + font-family: sans-serif; +} + +/* File view (all): "show/hide details" link format */ +a.detail:link +{ + color: #B8D0FF; + font-size:80%; +} + +/* File view (all): "show/hide details" link - visited format */ +a.detail:visited +{ + color: #B8D0FF; + font-size:80%; +} + +/* File view (all): "show/hide details" link - activated format */ +a.detail:active +{ + color: #FFFFFF; + font-size:80%; +} + +/* File view (detail): test name entry */ +td.testName +{ + text-align: right; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* File view (detail): test percentage entry */ +td.testPer +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* File view (detail): test lines count entry */ +td.testNum +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* Test case descriptions: test name format*/ +dt +{ + font-family: sans-serif; + font-weight: bold; +} + +/* Test case descriptions: description table body */ +td.testDescription +{ + padding-top: 10px; + padding-left: 30px; + padding-bottom: 10px; + padding-right: 30px; + background-color: #DAE7FE; +} + +/* Source code view: function entry */ +td.coverFn +{ + text-align: left; + padding-left: 10px; + padding-right: 20px; + color: #284FA8; + background-color: #DAE7FE; + font-family: monospace; +} + +/* Source code view: function entry zero count*/ +td.coverFnLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + font-weight: bold; + font-family: sans-serif; +} + +/* Source code view: function entry nonzero count*/ +td.coverFnHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-weight: bold; + font-family: sans-serif; +} + +/* Source code view: source code format */ +pre.source +{ + font-family: monospace; + white-space: pre; + margin-top: 2px; +} + +/* Source code view: line number format */ +span.lineNum +{ + background-color: #EFE383; +} + +/* Source code view: format for lines which were executed */ +td.lineCov, +span.lineCov +{ + background-color: #CAD7FE; +} + +/* Source code view: format for Cov legend */ +span.coverLegendCov +{ + padding-left: 10px; + padding-right: 10px; + padding-bottom: 2px; + background-color: #CAD7FE; +} + +/* Source code view: format for lines which were not executed */ +td.lineNoCov, +span.lineNoCov +{ + background-color: #FF6230; +} + +/* Source code view: format for NoCov legend */ +span.coverLegendNoCov +{ + padding-left: 10px; + padding-right: 10px; + padding-bottom: 2px; + background-color: #FF6230; +} + +/* Source code view (function table): standard link - visited format */ +td.lineNoCov > a:visited, +td.lineCov > a:visited +{ + color: black; + text-decoration: underline; +} + +/* Source code view: format for lines which were executed only in a + previous version */ +span.lineDiffCov +{ + background-color: #B5F7AF; +} + +/* Source code view: format for branches which were executed + * and taken */ +span.branchCov +{ + background-color: #CAD7FE; +} + +/* Source code view: format for branches which were executed + * but not taken */ +span.branchNoCov +{ + background-color: #FF6230; +} + +/* Source code view: format for branches which were not executed */ +span.branchNoExec +{ + background-color: #FF6230; +} + +/* Source code view: format for the source code heading line */ +pre.sourceHeading +{ + white-space: pre; + font-family: monospace; + font-weight: bold; + margin: 0px; +} + +/* All views: header legend value for low rate */ +td.headerValueLegL +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 4px; + padding-right: 2px; + background-color: #FF0000; + font-size: 80%; +} + +/* All views: header legend value for med rate */ +td.headerValueLegM +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 2px; + padding-right: 2px; + background-color: #FFEA20; + font-size: 80%; +} + +/* All views: header legend value for hi rate */ +td.headerValueLegH +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 2px; + padding-right: 4px; + background-color: #A7FC9D; + font-size: 80%; +} + +/* All views except source code view: legend format for low coverage */ +span.coverLegendCovLo +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #FF0000; +} + +/* All views except source code view: legend format for med coverage */ +span.coverLegendCovMed +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #FFEA20; +} + +/* All views except source code view: legend format for hi coverage */ +span.coverLegendCovHi +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #A7FC9D; +} diff --git a/glass.png b/glass.png new file mode 100644 index 0000000000000000000000000000000000000000..e1abc00680a3093c49fdb775ae6bdb6764c95af2 GIT binary patch literal 167 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)gaEa{HEjtmSN`?>!lvI6;R0X`wF z|Ns97GD8ntt^-nxB|(0{3=Yq3q=7g|-tI089jvk*Kn`btM`SSr1Gf+eGhVt|_XjA* zUgGKN%6^Gmn4d%Ph(nkFP>9RZ#WAE}PI3Z}&BVayv3^M*kj3EX>gTe~DWM4f=_Dpv literal 0 HcmV?d00001 diff --git a/index-sort-f.html b/index-sort-f.html new file mode 100644 index 0000000000..dd40a1d98c --- /dev/null +++ b/index-sort-f.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141542138366.2 %
Date:2024-04-25 05:42:57Functions:2625416963.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.7%71.7%
+ 		71.7 %132 / 18450.0 %17 / 34
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44659.4 %828 / 1394
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98562.4 %843 / 1351
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_uav_managers/include/transform_manager +
48.5%48.5%
+ 		48.5 %193 / 39868.4 %13 / 19
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.3%53.3%
+ 		53.3 %407 / 76369.5 %66 / 95
mrs_uav_managers/src/control_manager/common +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
mrs_uav_managers/include/mrs_uav_managers/control_manager +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_managers/src/control_manager +
65.2%65.2%
+ 		65.2 %2422 / 371378.0 %96 / 123
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_lib/src/timer +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
mrs_uav_managers/src/estimation_manager +
69.9%69.9%
+ 		69.9 %437 / 62585.1 %40 / 47
mrs_uav_managers/src +
73.6%73.6%
+ 		73.6 %1265 / 171887.7 %64 / 73
mrs_uav_controllers/src +
80.5%80.5%
+ 		80.5 %1751 / 217487.9 %58 / 66
mrs_uav_trackers/src/mpc_tracker +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/profiler +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
mrs_uav_managers/src/transform_manager +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_controllers/include +
98.3%98.3%
+ 		98.3 %57 / 58100.0 %14 / 14
mrs_uav_autostart/src +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index-sort-l.html b/index-sort-l.html new file mode 100644 index 0000000000..b6df310dbb --- /dev/null +++ b/index-sort-l.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141542138366.2 %
Date:2024-04-25 05:42:57Functions:2625416963.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_lib/src/profiler +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_uav_managers/src/control_manager/common +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_uav_managers/include/transform_manager +
48.5%48.5%
+ 		48.5 %193 / 39868.4 %13 / 19
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.3%53.3%
+ 		53.3 %407 / 76369.5 %66 / 95
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_managers/include/mrs_uav_managers/control_manager +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_uav_managers/src/control_manager +
65.2%65.2%
+ 		65.2 %2422 / 371378.0 %96 / 123
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
mrs_uav_managers/src/estimation_manager +
69.9%69.9%
+ 		69.9 %437 / 62585.1 %40 / 47
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.7%71.7%
+ 		71.7 %132 / 18450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_uav_managers/src +
73.6%73.6%
+ 		73.6 %1265 / 171887.7 %64 / 73
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98562.4 %843 / 1351
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_uav_controllers/src +
80.5%80.5%
+ 		80.5 %1751 / 217487.9 %58 / 66
mrs_uav_managers/src/transform_manager +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
mrs_uav_trackers/src/mpc_tracker +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44659.4 %828 / 1394
mrs_lib/src/timer +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
mrs_uav_autostart/src +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_uav_controllers/include +
98.3%98.3%
+ 		98.3 %57 / 58100.0 %14 / 14
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index.html b/index.html new file mode 100644 index 0000000000..c318cd84a7 --- /dev/null +++ b/index.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141542138366.2 %
Date:2024-04-25 05:42:57Functions:2625416963.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98562.4 %843 / 1351
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44659.4 %828 / 1394
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_lib/src/profiler +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/src/timer +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_autostart/src +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
mrs_uav_controllers/include +
98.3%98.3%
+ 		98.3 %57 / 58100.0 %14 / 14
mrs_uav_controllers/src +
80.5%80.5%
+ 		80.5 %1751 / 217487.9 %58 / 66
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_managers/include/mrs_uav_managers/control_manager +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_uav_managers/include/transform_manager +
48.5%48.5%
+ 		48.5 %193 / 39868.4 %13 / 19
mrs_uav_managers/src +
73.6%73.6%
+ 		73.6 %1265 / 171887.7 %64 / 73
mrs_uav_managers/src/control_manager +
65.2%65.2%
+ 		65.2 %2422 / 371378.0 %96 / 123
mrs_uav_managers/src/control_manager/common +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
mrs_uav_managers/src/estimation_manager +
69.9%69.9%
+ 		69.9 %437 / 62585.1 %40 / 47
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_managers/src/transform_manager +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.3%53.3%
+ 		53.3 %407 / 76369.5 %66 / 95
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.7%71.7%
+ 		71.7 %132 / 18450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_trackers/src/mpc_tracker +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html new file mode 100644 index 0000000000..fef9676f67 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::AttitudeConverter::MathErrorException::what() const0
mrs_lib::AttitudeConverter::SetHeadingException::what() const0
auto mrs_lib::AttitudeConverter::get<0ul>()1
auto mrs_lib::AttitudeConverter::get<1ul>()1
auto mrs_lib::AttitudeConverter::get<2ul>()1
mrs_lib::AttitudeConverter::GetHeadingException::what() const1
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen9AngleAxisIT_EEIdEEv1
mrs_lib::AttitudeConverter::InvalidAttitudeException::what() const2
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen10QuaternionIT_Li0EEEIdEEv93399
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)287308
mrs_lib::AttitudeConverter::AttitudeConverter<double>(Eigen::AngleAxis<double>)581214
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.func.html b/mrs_lib/include/mrs_lib/attitude_converter.h.func.html new file mode 100644 index 0000000000..7e313341fb --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)287308
auto mrs_lib::AttitudeConverter::get<0ul>()1
auto mrs_lib::AttitudeConverter::get<1ul>()1
auto mrs_lib::AttitudeConverter::get<2ul>()1
mrs_lib::AttitudeConverter::AttitudeConverter<double>(Eigen::AngleAxis<double>)581214
mrs_lib::AttitudeConverter::MathErrorException::what() const0
mrs_lib::AttitudeConverter::GetHeadingException::what() const1
mrs_lib::AttitudeConverter::SetHeadingException::what() const0
mrs_lib::AttitudeConverter::InvalidAttitudeException::what() const2
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen10QuaternionIT_Li0EEEIdEEv93399
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen9AngleAxisIT_EEIdEEv1
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.frameset.html new file mode 100644 index 0000000000..30914dc7fa --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html new file mode 100644 index 0000000000..8fd5fc9b88 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html @@ -0,0 +1,617 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: TomasFormat
+       2             : /**
+       3             :  * @file attitude_converter.h
+       4             :  *
+       5             :  * @brief Conversions between various representations of object attitude in 3D.
+       6             :  * Supports Quaternions, Euler angles, Angle-axis and Rotational matrices from tf, tf2, Eigen and geometry_msgs libraries.
+       7             :  * The default Euler angle notation is the extrinsic RPY.
+       8             :  *
+       9             :  * @author Tomas Baca
+      10             :  */
+      11             : 
+      12             : #ifndef ATTITUDE_CONVERTER_H
+      13             : #define ATTITUDE_CONVERTER_H
+      14             : 
+      15             : #include <vector>
+      16             : #include <cmath>
+      17             : #include <Eigen/Dense>
+      18             : #include <tuple>
+      19             : 
+      20             : #include <tf2_ros/transform_listener.h>
+      21             : #include <tf2_ros/buffer.h>
+      22             : #include <tf2_eigen/tf2_eigen.h>
+      23             : #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+      24             : #include <tf/transform_datatypes.h>
+      25             : #include <tf_conversions/tf_eigen.h>
+      26             : 
+      27             : #include <mrs_lib/geometry/misc.h>
+      28             : 
+      29             : namespace mrs_lib
+      30             : {
+      31             : 
+      32             : // type of the object we are grasping
+      33             : typedef enum
+      34             : {
+      35             : 
+      36             :   RPY_INTRINSIC = 1,
+      37             :   RPY_EXTRINSIC = 2,
+      38             : 
+      39             : } RPY_convention_t;
+      40             : 
+      41             : /* class EulerAttitude //{ */
+      42             : 
+      43             : /**
+      44             :  * @brief A small class for storing the Euler angles.
+      45             :  */
+      46             : class EulerAttitude {
+      47             : public:
+      48             :   /**
+      49             :    * @brief A simple class for storing the Euler angles.
+      50             :    *
+      51             :    * @param roll
+      52             :    * @param pitch
+      53             :    * @param yaw
+      54             :    */
+      55             :   EulerAttitude(const double& roll, const double& pitch, const double& yaw);
+      56             : 
+      57             :   /**
+      58             :    * @brief get the roll angle
+      59             :    *
+      60             :    * @return roll
+      61             :    */
+      62             :   double roll(void) const;
+      63             : 
+      64             :   /**
+      65             :    * @brief get the pitch angle
+      66             :    *
+      67             :    * @return pitch
+      68             :    */
+      69             :   double pitch(void) const;
+      70             : 
+      71             :   /**
+      72             :    * @brief get the yaw angle
+      73             :    *
+      74             :    * @return yaw
+      75             :    */
+      76             :   double yaw(void) const;
+      77             : 
+      78             : private:
+      79             :   double roll_, pitch_, yaw_;
+      80             : };
+      81             : 
+      82             : //}
+      83             : 
+      84             : /* class Vector3Converter //{ */
+      85             : 
+      86             : /**
+      87             :  * @brief Converter of Vector3 representations. Instantiate it with any type of vector3 in constructor and convert it by assigning it to any other type of
+      88             :  * vector3 variable.
+      89             :  */
+      90             : class Vector3Converter {
+      91             : public:
+      92             :   /**
+      93             :    * @brief Constructor with tf2::Vector3
+      94             :    *
+      95             :    * @param vector3
+      96             :    */
+      97      287308 :   Vector3Converter(const tf2::Vector3& vector3) : vector3_(vector3){};
+      98             : 
+      99             :   /**
+     100             :    * @brief Constructor with Eigen::Vector3
+     101             :    *
+     102             :    * @param vector3
+     103             :    */
+     104             :   Vector3Converter(const Eigen::Vector3d& vector3);
+     105             : 
+     106             :   /**
+     107             :    * @brief Constructor with geometry_msgs::Vector3
+     108             :    *
+     109             :    * @param vector3
+     110             :    */
+     111             :   Vector3Converter(const geometry_msgs::Vector3& vector3);
+     112             : 
+     113             :   /**
+     114             :    * @brief Constructor with doubles: x, y, z
+     115             :    *
+     116             :    * @param x
+     117             :    * @param y
+     118             :    * @param z
+     119             :    */
+     120             :   Vector3Converter(const double& x, const double& y, const double& z);
+     121             : 
+     122             :   /**
+     123             :    * @brief typecast overloaded for tf2::Vector3
+     124             :    *
+     125             :    * @return vector3
+     126             :    */
+     127             :   operator tf2::Vector3() const;
+     128             : 
+     129             :   /**
+     130             :    * @brief typecast overloaded for Eigen::Vector3
+     131             :    *
+     132             :    * @return vector3
+     133             :    */
+     134             :   operator Eigen::Vector3d() const;
+     135             : 
+     136             :   /**
+     137             :    * @brief typecast overloaded for geometry_msgs::Vector3
+     138             :    *
+     139             :    * @return vector3
+     140             :    */
+     141             :   operator geometry_msgs::Vector3() const;
+     142             : 
+     143             : private:
+     144             :   tf2::Vector3 vector3_;
+     145             : };
+     146             : 
+     147             : //}
+     148             : 
+     149             : /**
+     150             :  * @brief The main convertor class. Instantiate with any type in constructor and get the value in any other type by assigning the instance to your variable,
+     151             :  * as: tf::Quaternion tf1_quaternion = AttitudeConverter(roll, pitch, yaw); All the default Euler angles are in the extrinsic RPY notation.
+     152             :  */
+     153             : class AttitudeConverter {
+     154             : public:
+     155             :   /* exceptions //{ */
+     156             : 
+     157             :   //! is thrown when calculating of heading is not possible due to atan2 exception
+     158             :   struct GetHeadingException : public std::exception
+     159             :   {
+     160           1 :     const char* what() const throw() {
+     161           1 :       return "AttitudeConverter: can not calculate the heading, the rotated x-axis is parallel to the world's z-axis";
+     162             :     }
+     163             :   };
+     164             : 
+     165             :   //! is thrown when math breaks
+     166             :   struct MathErrorException : public std::exception
+     167             :   {
+     168           0 :     const char* what() const throw() {
+     169           0 :       return "AttitudeConverter: math error";
+     170             :     }
+     171             :   };
+     172             : 
+     173             :   //! is thrown when the internal attitude becomes invalid
+     174             :   struct InvalidAttitudeException : public std::exception
+     175             :   {
+     176           2 :     const char* what() const throw() {
+     177           2 :       return "AttitudeConverter: invalid attitude, the input probably constains NaNs";
+     178             :     }
+     179             :   };
+     180             : 
+     181             :   //! is thrown when the Euler angle format is set wrongly
+     182             :   struct EulerFormatException : public std::exception
+     183             :   {
+     184             :     const char* what() const throw() {
+     185             :       return "AttitudeConverter: invalid Euler angle format";
+     186             :     }
+     187             :   };
+     188             : 
+     189             :   //! is thrown when the heading cannot be set to an existing attitude
+     190             :   struct SetHeadingException : public std::exception
+     191             :   {
+     192           0 :     const char* what() const throw() {
+     193           0 :       return "AttitudeConverter: cannot set the desired heading, the thrust vector's Z component is 0";
+     194             :     }
+     195             :   };
+     196             : 
+     197             :   //}
+     198             : 
+     199             :   /* constructors //{ */
+     200             : 
+     201             :   /**
+     202             :    * @brief Euler angles constructor
+     203             :    *
+     204             :    * @param roll
+     205             :    * @param pitch
+     206             :    * @param yaw
+     207             :    * @param format optional, Euler angle convention, {"extrinsic", "intrinsic"}, defaults to "extrinsic"
+     208             :    */
+     209    13904552 :   AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format = RPY_EXTRINSIC);
+     210             : 
+     211             :   /**
+     212             :    * @brief tf::Quaternion constructor
+     213             :    *
+     214             :    * @param quaternion tf::Quaternion quaternion
+     215             :    */
+     216             :   AttitudeConverter(const tf::Quaternion quaternion);
+     217             : 
+     218             :   /**
+     219             :    * @brief geometry_msgs::Quaternion constructor
+     220             :    *
+     221             :    * @param quaternion geometry_msgs::Quaternion quaternion
+     222             :    */
+     223             :   AttitudeConverter(const geometry_msgs::Quaternion quaternion);
+     224             : 
+     225             :   /**
+     226             :    * @brief mrs_lib::EulerAttitude constructor
+     227             :    *
+     228             :    * @param euler_attitude mrs_lib::EulerAttitude
+     229             :    */
+     230             :   AttitudeConverter(const mrs_lib::EulerAttitude& euler_attitude);
+     231             : 
+     232             :   /**
+     233             :    * @brief Eigen::Quaterniond constructor
+     234             :    *
+     235             :    * @param quaternion Eigen::Quaterniond quaternion
+     236             :    */
+     237             :   AttitudeConverter(const Eigen::Quaterniond quaternion);
+     238             : 
+     239             :   /**
+     240             :    * @brief Eigen::Matrix3d constructor
+     241             :    *
+     242             :    * @param matrix Eigen::Matrix3d rotational matrix
+     243             :    */
+     244             :   AttitudeConverter(const Eigen::Matrix3d matrix);
+     245             : 
+     246             :   /**
+     247             :    * @brief Eigen::AngleAxis constructor
+     248             :    *
+     249             :    * @tparam T angle-axis base type
+     250             :    * @param angle_axis Eigen::AngleAxis
+     251             :    */
+     252             :   template <class T>
+     253      581214 :   AttitudeConverter(const Eigen::AngleAxis<T> angle_axis) {
+     254      579658 :     double       angle = angle_axis.angle();
+     255      579546 :     tf2::Vector3 axis(angle_axis.axis()[0], angle_axis.axis()[1], angle_axis.axis()[2]);
+     256             : 
+     257      580358 :     tf2_quaternion_.setRotation(axis, angle);
+     258      579632 :   }
+     259             : 
+     260             :   /**
+     261             :    * @brief tf2::Quaternion constructor
+     262             :    *
+     263             :    * @param quaternion tf2::Quaternion
+     264             :    */
+     265             :   AttitudeConverter(const tf2::Quaternion quaternion);
+     266             : 
+     267             :   /**
+     268             :    * @brief tf2::Matrix3x3 constructor
+     269             :    *
+     270             :    * @param quaternion tf2::Matrix3x3
+     271             :    */
+     272             :   AttitudeConverter(const tf2::Matrix3x3 matrix);
+     273             : 
+     274             :   //}
+     275             : 
+     276             :   /* operators //{ */
+     277             : 
+     278             :   /**
+     279             :    * @brief typecast to tf2::Quaternion
+     280             :    *
+     281             :    * @return orientation in tf2::Quaternion
+     282             :    */
+     283             :   operator tf2::Quaternion() const;
+     284             : 
+     285             :   /**
+     286             :    * @brief typecast to tf::Quaternion
+     287             :    *
+     288             :    * @return orientation in tf::Quaternion
+     289             :    */
+     290             :   operator tf::Quaternion() const;
+     291             : 
+     292             :   /**
+     293             :    * @brief typecast to geometry_msgs::Quaternion
+     294             :    *
+     295             :    * @return orientation in geometry_msgs::Quaternion
+     296             :    */
+     297             :   operator geometry_msgs::Quaternion() const;
+     298             : 
+     299             :   /**
+     300             :    * @brief typecast to EulerAttitude
+     301             :    *
+     302             :    * @return orientation in EulerAttitude
+     303             :    */
+     304             :   operator EulerAttitude() const;
+     305             : 
+     306             :   /**
+     307             :    * @brief typecast to Eigen::AngleAxis
+     308             :    *
+     309             :    * @tparam T angle-axis base type
+     310             :    *
+     311             :    * @return orientation in EulerAttitude
+     312             :    */
+     313             :   template <class T>
+     314           1 :   operator Eigen::AngleAxis<T>() const {
+     315             : 
+     316           1 :     double          angle = tf2_quaternion_.getAngle();
+     317           1 :     Eigen::Vector3d axis(tf2_quaternion_.getAxis()[0], tf2_quaternion_.getAxis()[1], tf2_quaternion_.getAxis()[2]);
+     318             : 
+     319           1 :     Eigen::AngleAxis<T> angle_axis(angle, axis);
+     320             : 
+     321           2 :     return angle_axis;
+     322             :   }
+     323             : 
+     324             : 
+     325             :   /**
+     326             :    * @brief typecast to EulerAttitude Eigen::Quaternion
+     327             :    *
+     328             :    * @tparam T quaternion base type
+     329             :    *
+     330             :    * @return orientation in Eigen::Quaternion
+     331             :    */
+     332             :   template <class T>
+     333       93399 :   operator Eigen::Quaternion<T>() const {
+     334             : 
+     335       93399 :     return Eigen::Quaternion<T>(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     336             :   }
+     337             : 
+     338             :   operator Eigen::Matrix3d() const;
+     339             : 
+     340             :   /**
+     341             :    * @brief typecase to tuple of Euler angles in extrinsic RPY
+     342             :    *
+     343             :    * @return std::tuple of extrinsic RPY
+     344             :    */
+     345             :   operator std::tuple<double&, double&, double&>();
+     346             : 
+     347             :   /**
+     348             :    * @brief typecase to tf2::Matrix3x3
+     349             :    *
+     350             :    * @return tf2::Matrix3x3 rotational matrix
+     351             :    */
+     352             :   operator tf2::Matrix3x3() const;
+     353             : 
+     354             :   /**
+     355             :    * @brief typecase to tf2::Transform
+     356             :    *
+     357             :    * @return tf2::Transform
+     358             :    */
+     359             :   operator tf2::Transform() const;
+     360             : 
+     361             :   //}
+     362             : 
+     363             :   /* getters //{ */
+     364             : 
+     365             :   /**
+     366             :    * @brief get the roll angle
+     367             :    *
+     368             :    * @return roll
+     369             :    */
+     370             :   double getRoll(void);
+     371             : 
+     372             :   /**
+     373             :    * @brief get the pitch angle
+     374             :    *
+     375             :    * @return pitch
+     376             :    */
+     377             :   double getPitch(void);
+     378             : 
+     379             :   /**
+     380             :    * @brief get the yaw angle
+     381             :    *
+     382             :    * @return yaw
+     383             :    */
+     384             :   double getYaw(void);
+     385             : 
+     386             :   /**
+     387             :    * @brief get the angle of the rotated x-axis in the original XY plane, a.k.a
+     388             :    *
+     389             :    * @return heading
+     390             :    */
+     391             :   double getHeading(void);
+     392             : 
+     393             :   /**
+     394             :    * @brief get heading rate base on the orientation and body-based attitude rate
+     395             :    *
+     396             :    * @param attitude_rate in the body frame
+     397             :    *
+     398             :    * @return heading rate in the world
+     399             :    */
+     400             :   double getHeadingRate(const Vector3Converter& attitude_rate);
+     401             : 
+     402             :   /**
+     403             :    * @brief get the intrinsic yaw rate from a heading rate
+     404             :    *
+     405             :    * @param heading_rate
+     406             :    *
+     407             :    * @return intrinsic yaw rate
+     408             :    */
+     409             :   double getYawRateIntrinsic(const double& heading_rate);
+     410             : 
+     411             :   /**
+     412             :    * @brief get a unit vector pointing in the X direction
+     413             :    *
+     414             :    * @return the vector
+     415             :    */
+     416             :   Vector3Converter getVectorX(void);
+     417             : 
+     418             :   /**
+     419             :    * @brief get a unit vector pointing in the Y direction
+     420             :    *
+     421             :    * @return the vector
+     422             :    */
+     423             :   Vector3Converter getVectorY(void);
+     424             : 
+     425             :   /**
+     426             :    * @brief get a unit vector pointing in the Z direction
+     427             :    *
+     428             :    * @return the vector
+     429             :    */
+     430             :   Vector3Converter getVectorZ(void);
+     431             : 
+     432             :   /**
+     433             :    * @brief get the Roll, Pitch, Yaw angles in the Intrinsic convention
+     434             :    *
+     435             :    * @return RPY
+     436             :    */
+     437             :   std::tuple<double, double, double> getIntrinsicRPY();
+     438             : 
+     439             :   /**
+     440             :    * @brief get the Roll, Pitch, Yaw angles in the Extrinsic convention. The same as the default AttitudeConverter assignment.
+     441             :    *
+     442             :    * @return RPY
+     443             :    */
+     444             :   std::tuple<double, double, double> getExtrinsicRPY();
+     445             : 
+     446             :   //}
+     447             : 
+     448             :   /* setters //{ */
+     449             : 
+     450             :   /**
+     451             :    * @brief Updates the heading of the current orientation by updating the intrinsic yaw
+     452             :    *
+     453             :    * @param new heading
+     454             :    *
+     455             :    * @return the orientation
+     456             :    */
+     457             :   AttitudeConverter setHeading(const double& heading);
+     458             : 
+     459             :   /**
+     460             :    * @brief Updates the extrinsic yaw of the current orientation.
+     461             :    *
+     462             :    * @param new yaw
+     463             :    *
+     464             :    * @return the orientation
+     465             :    */
+     466             :   AttitudeConverter setYaw(const double& new_yaw);
+     467             : 
+     468             :   //}
+     469             : 
+     470             :   template <std::size_t I>
+     471             :   constexpr auto get();
+     472             : 
+     473             : private:
+     474             :   /**
+     475             :    * @brief Internal representation of the attitude
+     476             :    */
+     477             :   tf2::Quaternion tf2_quaternion_;
+     478             : 
+     479             :   /**
+     480             :    * @brief convert the internal quaternion representation to internally-stored RPY
+     481             :    */
+     482             :   void calculateRPY(void);
+     483             : 
+     484             :   /**
+     485             :    * @brief throws exception when the internal attitude is invalid
+     486             :    */
+     487             :   void validateOrientation(void);
+     488             : 
+     489             :   /**
+     490             :    * @brief Internal representation in RPY. is used only when converting to RPY.
+     491             :    */
+     492             :   double roll_, pitch_, yaw_;
+     493             :   bool   got_rpy_ = false;
+     494             : };
+     495             : 
+     496             : 
+     497             : template <std::size_t I>
+     498           3 : constexpr auto AttitudeConverter::get() {
+     499             : 
+     500           3 :   calculateRPY();
+     501             : 
+     502             :   // call compilation error if I > 2
+     503             :   static_assert(I <= 2);
+     504             : 
+     505             :   // get the RPY components based on the index in the tuple
+     506             :   if constexpr (I == 0) {
+     507           1 :     return static_cast<double>(roll_);
+     508             :   } else if constexpr (I == 1) {
+     509           1 :     return static_cast<double>(pitch_);
+     510             :   } else if constexpr (I == 2) {
+     511           1 :     return static_cast<double>(yaw_);
+     512             :   }
+     513             : }
+     514             : 
+     515             : }  // namespace mrs_lib
+     516             : 
+     517             : template <>
+     518             : struct std::tuple_size<mrs_lib::AttitudeConverter>
+     519             : { static constexpr int value = 3; };
+     520             : 
+     521             : template <>
+     522             : struct std::tuple_element<0, mrs_lib::AttitudeConverter>
+     523             : { using type = double; };
+     524             : 
+     525             : template <>
+     526             : struct std::tuple_element<1, mrs_lib::AttitudeConverter>
+     527             : { using type = double; };
+     528             : 
+     529             : template <>
+     530             : struct std::tuple_element<2, mrs_lib::AttitudeConverter>
+     531             : { using type = double; };
+     532             : 
+     533             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html new file mode 100644 index 0000000000..2b64ca38c0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html @@ -0,0 +1,154 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5645ea2bb184ba5c13faadff6ae080a7ed21a006 GIT binary patch literal 1675 zcmV;626Xv}P)R0{{R3v^8aD0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp^O~xJw}!I*vg`@baJIpOoIavV`$(K?>!`bo|a^C) za8g(t(GB3SHwi*Y*B(ABA)4cq7R?deK-XjKEeJ~&vr9LR}k6~|SE zj-$qCkGRR)tetw3kCxx*!;6UEC@RHf4BS;>N2h>L$)pCW@@QNm!OX>*`BO)Lk71D%>OnE>K^hjQTBMES? zGvhv}xdH-50z4|BgOy^!kpa(GH^`~foRA8S^Y>UW#vJMJOe}{A4}y>ij~qH^H4MF) zneqsxd6A=h9u5{hcDe;P(zMKk1rd-Ue&^knPPR3jA9^gb>#8x~Ik7N~R1Fdq@=|ju z0T!S8M{bY&wr%zW06bfshHfN{MDlGm(rkM>62Wo>ikiawavEnFLStBy1HdIWEFe%Ww^Ngwm;Hcq)jl z0*{0W;oTtCZ~llxle+lG>3#O+D&&gJrG@=)lao1h2seK7gq0SK9kjuxg-NrKGR!%~ z`au99dU_T5N&6~=-sZqq>k<~8`<@qW?#HHsHY0qrv%R~ejG3dmdiYQNZF4O4*G{-$ z^G7N8QUf@SO&a1DH?d4h1F$$gpRd=K`+0r3pV#NJ)po+CrwQ%T-SgEVtiNn>JQv4Q z$!a>`qaW=;`3?H`0rb(Ur2TajeM}|3XxAlSQK}p$B;JRoy~&s`4k)2hNq1Dm2(-&Z z%ZP0UeT*pTteufi+CMJX^?uWrH*Wvgee-&=U5j%JyB%`DkeX;_W9brNyMAW@LV=NF z`vbMAFhtINPYBIhx=Oy)`A1x1t=K11!#H_Sq=yYc#`?Mxm2Dy{cz^))?R~`tEg7 zo(>_l>s;xc@cdbu``xNXAjEdvf~QT$fk&n`v;a?Mx|iT-@?_UO)>ViPPuH7ayKcbK zIe5W=N5r<4w5LOe?HZq((Kwy}I4+3;`PQSeHB3yo++}$V=9qWo+^i(35sOuwsIM>rafH+-<*caYfll7-k8qBGOLd1CnUsn zot2`+1`_h(bt-s#>%p`$vc0^nk+7M>5i7}n<3->Z68->q@ErZ91s_QKmCsa&B%_`q z1D@+YQz7;U=C6FFLhb=op7xZL`xqZRZtpz_n1U--iU~*Zp}2TpDV}{(xy0affm7%5 z+W7_^4dR!LZX*O5l2^DZ1 z?A~WeTGw4u;hC=z$ot`PUf>U&F~>i6h8%ghnJNAcf#T%Hdy|jS=_H)EsjMJ8?!*5I z51!+@7iT$1QUD{R;|$UhBs~Y1;Zi2XPgaWJl!jXIElj|TK46~NkO7}ca2>#x;XcI* z^CV0j8!}-zAteJI3FZ@GU`1FF5~pzC_+7=3X)_ASQIrJ;gUR_iSNsD<>R>{U1Y^?W zvYR!Sg}4SeuBe%R`Cnmz$V;iPVjSJHb9JE2c3kC8_@f)zqlD5AEDo&}n_5NIC62c= zqkgYRb;F6tx*!Fl3jb2?u8vgi2?nd`J-wb + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - dynamic_reconfigure_mgr.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:578765.5 %
Date:2024-04-25 05:42:57Functions:288532.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<int>(boost::any&, int*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<bool>(boost::any&, bool*&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<int>(boost::any&, int*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_defaults(mrs_uav_state_estimators::HeadingEstimatorConfig&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::update_config(mrs_uav_state_estimators::HeadingEstimatorConfig const&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::HeadingEstimatorConfig const&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::HeadingEstimatorConfig&, unsigned int)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<int>(boost::any&, int*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::HeadingEstimatorConfig&, unsigned int)> const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<int>(boost::any&, int*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<bool>(boost::any&, bool*&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<int>(boost::any&, int*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<bool>(boost::any&, bool*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_defaults(mrs_uav_state_estimators::LateralEstimatorConfig&)99
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)99
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)99
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)> const&)99
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_defaults(mrs_uav_state_estimators::AltitudeEstimatorConfig&)172
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)172
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)172
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)> const&)172
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)198
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)297
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)297
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<double>(boost::any&, double*&)297
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)297
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)344
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)354
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_defaults(mrs_uav_state_estimators::CorrectionConfig&)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)354
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<double>(boost::any&, double*&)354
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::CorrectionConfig&, unsigned int)> const&)354
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)516
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)516
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<double>(boost::any&, double*&)516
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)516
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)708
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html new file mode 100644 index 0000000000..62518a34a3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html @@ -0,0 +1,420 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - dynamic_reconfigure_mgr.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:578765.5 %
Date:2024-04-25 05:42:57Functions:288532.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)354
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::CorrectionConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::CorrectionConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)354
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_compare<int>(boost::any&, int*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_defaults(mrs_uav_state_estimators::CorrectionConfig&)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)708
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)354
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<double>(boost::any&, double*&)354
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)354
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::CorrectionConfig&, unsigned int)> const&)354
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::HeadingEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::HeadingEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_compare<int>(boost::any&, int*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::load_defaults(mrs_uav_state_estimators::HeadingEstimatorConfig&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::update_config(mrs_uav_state_estimators::HeadingEstimatorConfig const&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::HeadingEstimatorConfig const&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::HeadingEstimatorConfig&, unsigned int)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::try_cast<int>(boost::any&, int*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::HeadingEstimatorConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::HeadingEstimatorConfig&, unsigned int)> const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)297
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::LateralEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::LateralEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)297
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_compare<int>(boost::any&, int*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::load_defaults(mrs_uav_state_estimators::LateralEstimatorConfig&)99
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)198
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)99
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)99
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<double>(boost::any&, double*&)297
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)297
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)> const&)99
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)516
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_param<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<mrs_uav_state_estimators::AltitudeEstimatorConfig::AbstractParamDescription const>&, mrs_uav_state_estimators::AltitudeEstimatorConfig&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)0
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)516
void mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_value<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<double>(boost::any&, double*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_compare<int>(boost::any&, int*&)0
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_defaults(mrs_uav_state_estimators::AltitudeEstimatorConfig&)172
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)344
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)172
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)172
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(boost::any&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<bool>(boost::any&, bool*&)0
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<double>(boost::any&, double*&)516
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)516
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::function<void (mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)> const&)172
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.frameset.html new file mode 100644 index 0000000000..fa0e06e3c7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html new file mode 100644 index 0000000000..726108f555 --- /dev/null +++ b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html @@ -0,0 +1,343 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - dynamic_reconfigure_mgr.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:578765.5 %
Date:2024-04-25 05:42:57Functions:288532.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines DynamicReconfigureMgr - a convenience class for managing dynamic ROS parameters through dynamic reconfigure.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : #ifndef DYNAMIC_RECONFIGURE_MGR_H
+       7             : #define DYNAMIC_RECONFIGURE_MGR_H
+       8             : 
+       9             : #include <ros/ros.h>
+      10             : #include <dynamic_reconfigure/server.h>
+      11             : #include <string>
+      12             : #include <map>
+      13             : #include <unordered_set>
+      14             : #include <mutex>
+      15             : #include <iostream>
+      16             : #include <boost/any.hpp>
+      17             : #include <Eigen/Dense>
+      18             : #include <mrs_lib/param_loader.h>
+      19             : 
+      20             : 
+      21             : namespace mrs_lib
+      22             : {
+      23             : 
+      24             : /** DynamicReconfigureMgr CLASS //{ **/
+      25             : // This class handles dynamic reconfiguration of parameters using dynamic_reconfigure server.
+      26             : // Initialize this manager simply by instantiating an object of this templated class
+      27             : // with the template parameter corresponding to the type of your config message, e.g. as
+      28             : // DynamicReconfigureMgr<MyConfig> drmgr;
+      29             : // This will automatically initialize the dynamic_reconfigure server and a callback method
+      30             : // to asynchronously update the values in the config.
+      31             : // Optionally, you can specify the ros NodeHandle to initialize the dynamic_reconfigure server
+      32             : // and a flag 'print_values' to indicate whether to print new received values (only changed ones,
+      33             : // default is true).
+      34             : // The latest configuration is available through the public member 'config'. This should be
+      35             : // changed externally with care since any change risks being overwritten in the next call to
+      36             : // the 'dynamic_reconfigure_callback' method.
+      37             : // Note that in case of a multithreaded ROS node, external mutexes _might_ be necessary
+      38             : // to make access to the 'config' member thread-safe.
+      39             : template <typename ConfigType>
+      40             : class DynamicReconfigureMgr
+      41             : {
+      42             :   private:
+      43             :     using callback_t = typename dynamic_reconfigure::Server<ConfigType>::CallbackType;
+      44             : public:
+      45             :   // this variable holds the latest received configuration
+      46             :   ConfigType config;
+      47             :   // initialize some stuff in the constructor
+      48         625 :   DynamicReconfigureMgr(const ros::NodeHandle& nh = ros::NodeHandle("~"), bool print_values = true, std::string node_name = std::string(), const callback_t& user_callback = {})
+      49             :       : m_not_initialized(true),
+      50             :         m_loaded_invalid_default(false),
+      51             :         m_print_values(print_values),
+      52             :         m_node_name(node_name),
+      53         625 :         m_server(m_server_mtx, nh),
+      54             :         m_usr_cbf(user_callback),
+      55         625 :         m_pl(nh, print_values, node_name)
+      56             :   {
+      57             :     // initialize the dynamic reconfigure callback
+      58         625 :     m_server.setCallback(boost::bind(&DynamicReconfigureMgr<ConfigType>::dynamic_reconfigure_callback, this, _1, _2));
+      59         625 :   };
+      60             : 
+      61             :   /* Constructor overloads //{ */
+      62             :   // Convenience constructor to enable writing DynamicReconfigureMgr<MyConfig> drmgr(nh, node_name)
+      63         354 :   DynamicReconfigureMgr(const ros::NodeHandle& nh, std::string node_name) : DynamicReconfigureMgr(nh, true, node_name){};
+      64             :   // Convenience constructor to enable writing DynamicReconfigureMgr<MyConfig> drmgr(nh, "node_name")
+      65             :   DynamicReconfigureMgr(const ros::NodeHandle& nh, const char* node_name) : DynamicReconfigureMgr(nh, std::string(node_name)){};
+      66             :   // Convenience constructor to enable writing DynamicReconfigureMgr<MyConfig> drmgr(node_name)
+      67             :   DynamicReconfigureMgr(std::string node_name) : DynamicReconfigureMgr(ros::NodeHandle("~"), node_name){};
+      68             :   //}
+      69             : 
+      70             :   // pushes this config to the server
+      71        1250 :   void update_config(const ConfigType& cfg)
+      72             :   {
+      73        1250 :     m_server.updateConfig(cfg);
+      74        1250 :   }
+      75             : 
+      76             :   // pushes the current config to the server
+      77             :   void update_config()
+      78             :   {
+      79             :     m_server.updateConfig(config);
+      80             :   }
+      81             : 
+      82             :   void publish_descriptions()
+      83             :   {
+      84             :     ConfigType dflt;
+      85             :     m_server.getConfigDefault(dflt);
+      86             :     m_server.setConfigDefault(dflt);
+      87             :   }
+      88             : 
+      89             :   bool loaded_successfully()
+      90             :   {
+      91             :     return !m_not_initialized && !m_loaded_invalid_default && m_pl.loadedSuccessfully();
+      92             :   }
+      93             : 
+      94             : private:
+      95             :   bool m_not_initialized, m_loaded_invalid_default, m_print_values;
+      96             :   std::string m_node_name;
+      97             :   // dynamic_reconfigure server variables
+      98             :   boost::recursive_mutex m_server_mtx;
+      99             :   typename dynamic_reconfigure::Server<ConfigType> m_server;
+     100             :   callback_t m_usr_cbf;
+     101             : 
+     102             :   ParamLoader m_pl;
+     103             :   std::unordered_set<std::string> m_to_init;
+     104             : 
+     105             :   // the callback itself
+     106         625 :   void dynamic_reconfigure_callback(ConfigType& new_config, uint32_t level)
+     107             :   {
+     108         625 :     if (m_print_values)
+     109             :     {
+     110         625 :       if (m_node_name.empty())
+     111           0 :         ROS_INFO("Dynamic reconfigure request received");
+     112             :       else
+     113         625 :         ROS_INFO("[%s]: Dynamic reconfigure request received", m_node_name.c_str());
+     114             :     }
+     115             : 
+     116         625 :     if (m_not_initialized)
+     117             :     {
+     118         625 :       load_defaults(new_config);
+     119         625 :       update_config(new_config);
+     120             :     }
+     121         625 :     if (m_print_values)
+     122             :     {
+     123         625 :       print_changed_params(new_config);
+     124             :     }
+     125         625 :     m_not_initialized = false;
+     126         625 :     config = new_config;
+     127         625 :     if (m_usr_cbf)
+     128         271 :       m_usr_cbf(new_config, level);
+     129         625 :   }
+     130             : 
+     131             :   template <typename T>
+     132        1167 :   void load_param(const std::string& name, typename ConfigType::AbstractParamDescriptionConstPtr& descr, ConfigType& config)
+     133             :   {
+     134             :     using param_descr_t = typename ConfigType::template ParamDescription<T>;
+     135        2334 :     boost::shared_ptr<const param_descr_t> cast_descr = boost::dynamic_pointer_cast<const param_descr_t>(descr);
+     136        1167 :     m_pl.loadParam(name, config.*(cast_descr->field));
+     137        1167 :   }
+     138             :   
+     139         625 :   void load_defaults(ConfigType& new_config)
+     140             :   {
+     141             :     // Note that this part of the API is still unstable and may change! It was tested with ROS Kinetic and Melodic.
+     142        1250 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     143        1792 :     for (auto& descr : descrs)
+     144             :     {
+     145        2334 :       std::string name = descr->name;
+     146        1167 :       size_t pos = name.find("__");
+     147        1167 :       while (pos != name.npos)
+     148             :       {
+     149           0 :         name.replace(pos, 2, "/");
+     150           0 :         pos = name.find("__");
+     151             :       }
+     152             : 
+     153        1167 :       if (descr->type == "bool")
+     154           0 :         load_param<bool>(name, descr, new_config);
+     155        1167 :       else if (descr->type == "int")
+     156           0 :         load_param<int>(name, descr, new_config);
+     157        1167 :       else if (descr->type == "double")
+     158        1167 :         load_param<double>(name, descr, new_config);
+     159           0 :       else if (descr->type == "str")
+     160           0 :         load_param<std::string>(name, descr, new_config);
+     161             :       else
+     162             :       {
+     163           0 :         ROS_ERROR("[%s]: Unknown parameter type: '%s'", m_node_name.c_str(), descr->type.c_str());
+     164           0 :         m_loaded_invalid_default = true;
+     165             :       }
+     166             :     }
+     167         625 :   }
+     168             : 
+     169             :   // method for printing names and values of new received parameters (prints only the changed ones) //{
+     170         625 :   void print_changed_params(const ConfigType& new_config)
+     171             :   {
+     172             :     // Note that this part of the API is still unstable and may change! It was tested with ROS Kinetic and Melodic.
+     173        1250 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     174        1792 :     for (auto& descr : descrs)
+     175             :     {
+     176           0 :       boost::any val, old_val;
+     177        1167 :       descr->getValue(new_config, val);
+     178        1167 :       descr->getValue(config, old_val);
+     179        1167 :       std::string name = descr->name;
+     180        1167 :       const size_t pos = name.find("__");
+     181        1167 :       if (pos != name.npos)
+     182             :       {
+     183           0 :         if (m_not_initialized)
+     184             :         {
+     185           0 :           continue;
+     186             :         } else
+     187             :         {
+     188           0 :           name.replace(pos, 2, "/");
+     189             :         }
+     190             :       }
+     191             : 
+     192             :       // try to guess the correct type of the parameter (these should be the only ones supported)
+     193             :       int* intval;
+     194             :       double* doubleval;
+     195             :       bool* boolval;
+     196             :       std::string* stringval;
+     197             : 
+     198        1167 :       if (try_cast(val, intval))
+     199             :       {
+     200           0 :         if (m_not_initialized || !try_compare(old_val, intval))
+     201           0 :           print_value(name, *intval);
+     202        1167 :       } else if (try_cast(val, doubleval))
+     203             :       {
+     204        1167 :         if (m_not_initialized || !try_compare(old_val, doubleval))
+     205        1167 :           print_value(name, *doubleval);
+     206           0 :       } else if (try_cast(val, boolval))
+     207             :       {
+     208           0 :         if (m_not_initialized || !try_compare(old_val, boolval))
+     209           0 :           print_value(name, *boolval);
+     210           0 :       } else if (try_cast(val, stringval))
+     211             :       {
+     212           0 :         if (m_not_initialized || !try_compare(old_val, stringval))
+     213           0 :           print_value(name, *stringval);
+     214             :       } else
+     215             :       {
+     216           0 :         print_value(name, std::string("unknown dynamic reconfigure type"));
+     217             :       }
+     218             :     }
+     219         625 :   }
+     220             :   //}
+     221             :   
+     222             :   // helper method for parameter printing
+     223             :   template <typename T>
+     224        1167 :   inline void print_value(const std::string& name, const T& val)
+     225             :   {
+     226        1167 :     if (m_node_name.empty())
+     227           0 :       std::cout << "\t" << name << ":\t" << val << std::endl;
+     228             :     else
+     229        1167 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << val);
+     230        1167 :   }
+     231             :   // helper methods for automatic parameter value parsing
+     232             :   template <typename T>
+     233        2334 :   inline bool try_cast(boost::any& val, T*& out)
+     234             :   {
+     235        2334 :     return (out = boost::any_cast<T>(&val));
+     236             :   }
+     237             :   template <typename T>
+     238           0 :   inline bool try_compare(boost::any& val, T*& to_what)
+     239             :   {
+     240             :     T* tmp;
+     241           0 :     if ((tmp = boost::any_cast<T>(&val)))
+     242             :     {
+     243             :       /* std::cout << std::endl << *tmp << " vs " << *to_what << std::endl; */
+     244           0 :       return *tmp == *to_what;
+     245             :     } else
+     246             :     {  // the value should not change during runtime - this should never happen (but its better to be safe than sorry)
+     247           0 :       if (m_node_name.empty())
+     248           0 :         ROS_WARN("DynamicReconfigure value type has changed - this should not happen!");
+     249             :       else
+     250           0 :         ROS_WARN_STREAM("[" << m_node_name << "]: DynamicReconfigure value type has changed - this should not happen!");
+     251           0 :       return false;
+     252             :     }
+     253             :   }
+     254             : };
+     255             : //}
+     256             : 
+     257             : }
+     258             : 
+     259             : #endif // DYNAMIC_RECONFIGURE_MGR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.overview.html b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.overview.html new file mode 100644 index 0000000000..d3923d0459 --- /dev/null +++ b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.overview.html @@ -0,0 +1,85 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.png b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..beea652aa5ace2e35921803bbdef95ad964a6e86 GIT binary patch literal 1370 zcmV-g1*Q6lP)WHHeNXCF(^#PvpXJ0r5 za(hJuf(6Erkyl(*bo2zG;(+EIgWhM3fw5Ww$!+-pGoSzJYqc9oUExez$t=$+)bZ+Q zbeh+mmC{gsM8h!R(9z&@66o%wJv^?vpld` z1mwUy_EZ=P>cGr&HBSB;hUI}7>#aS*TkW# zLRC%f3f+?;%m(&YP^WyRV*E*pjtx-5RbWJp3fQe0i>rHhNuciJ>LaP4PRA4)McZJf zj#SR_)1VFFKq%Zba*4beX5iT3*OZN6BS#p*+t?T#o6w;o^t_y%Maiz?C2Coc%V2(f z&4;9~B~p>Fa#x_II-zHe$0p3wpWWeT1iHGavX!;#9$OE0x7v$I{g|U&GK;9^mM&8( z-iG+cz%RRJ1ia-Q;u&P-`@m#9pVsq;B_z}EQiE8W@X;~HYA1FS`hKqh2YeYqkZ*re zN1yU2T5Z~sUflPY=-RRlaqD%Js%u?>dYx{T<~S?M0JO0@OEXte2fhw^1MTz-?_QN$ z#=YjmTdpMBlGs|7+Zn4dW)em}=0{FX0L`Mm5fp9Eeyk%hO2aVAz!+x}!>pc*X2vAl zNndGaub|*X25idOxBb`_Qf=aI z?z!;LBe$6W_XvR+P=9VybgS8LN+%GuCOLXXR^QCRLL!;R@710X zPa`g6FY#v~`hO~WIMrK;0euSB_Ml>rGp!fLKwHI4^P&s2C+Ww840xmBZj=eSdtSy| z$iP>xDs?#;N`Nz001`b}KkUZX6Z;0`6Hj(9g~#V%VFz>>XuDR)uKLb_-J{?x$0xVg zYu#0(QLpgZz};C4=eIPwZBVFz2^S!uXj*%U+I3u4zB5d`^a<{z*vq=yb&)G6gX?fH zT8nVo$~E!hTL7CayUOvRV-NbAj&BZZbkyBr=|0Zbz2AK7QooHy<)8Kx;~jkk+NoXx zrmL#sn?4MC*Sy5ZWGMUBGdHU-zc}R@wG`AU?73Gck28!R-s+?|W9KS+3QYgQhybKLXL8s?w+3$p8QV07*qoM6N<$f;1+n!vFvP literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html b/mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html new file mode 100644 index 0000000000..d5c4c30cbd --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-04-25 05:42:57Functions:4811741.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::sdegrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::sradians const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::value() const0
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::convert<mrs_lib::geometry::radians>() const1
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::value() const4
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>&&)6
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::value() const10002
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10004
mrs_lib::geometry::operator-(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10009
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)10010
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10374
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(double)20000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::wrap(double)30000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)40000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::value() const40002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)403660
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)480731
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)654280
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)654280
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)654280
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)785709
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)828549
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)895238
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)1328588
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1820217
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1842673
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)3094649
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)3108297
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)3772818
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)7645639
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)8420903
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html b/mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html new file mode 100644 index 0000000000..363c62946e --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-04-25 05:42:57Functions:4811741.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)895238
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)480731
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)403660
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10374
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1842673
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)785709
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)828549
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1820217
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>&&)6
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::wrap(double)30000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::sdegrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(double)20000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)654280
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)654280
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)3772818
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)3108297
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)10010
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)8420903
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)654280
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)3094649
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::sradians const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)1328588
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)7645639
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
mrs_lib::geometry::operator-(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10009
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10004
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)40000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::value() const4
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::value() const10002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::value() const0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::value() const40002
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::convert<mrs_lib::geometry::radians>() const1
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.frameset.html new file mode 100644 index 0000000000..380619f2cf --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html new file mode 100644 index 0000000000..97e7c28eae --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-04-25 05:42:57Functions:4811741.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines the cyclic class for calculations with cyclic quantities.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef CYCLIC_H
+       8             : #define CYCLIC_H
+       9             : 
+      10             : #include <cmath>
+      11             : #include <ostream>
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15             :   namespace geometry
+      16             :   {
+      17             :     /**
+      18             :      * \brief Implementation of the a general cyclic value (such as angles in radians/degrees etc).
+      19             :      *
+      20             :      * This class implements a periodical value with a period of \f$ r \in {\rm I\!R} \f$ so that a general number \f$ v \in {\rm I\!R} \f$ represents the same
+      21             :      * value as \f$ v + kr,~k \in {\rm I\!N} \f$. For the purposes of calculations in this class, \f$ v \f$ is confined to a half-open interval \f$ v \in
+      22             :      * [~m,~s~[ \f$, where \f$ m \f$ is the minimum of this interval and \f$ s \f$ is its supremum, and \f$ s = m + r \f$ holds. This approach enables
+      23             :      * representing \f$ v \f$ in different intervals on the real numbers axis; eg. angle in radians may be represented within the interval \f$ v \in
+      24             :      * [~-\pi,~\pi~[ \f$ or \f$ v \in [~0,~2\pi~[ \f$, according to the needs of the specific application. The period \f$ r \f$ is called \p range for the
+      25             :      * purposes of this class, as it represents range of the interval of valid ("wrapped") values of \f$ v \f$.
+      26             :      *
+      27             :      * This class may be used as an object or its static methods may be used on regular floating-point types, avoiding any object-related overheads (see
+      28             :      * example). Specializations for the most common cyclic values are provided and a new specialization may be easily created simply by inheriting from this
+      29             :      * class and specifying a different minimum and supremum values.
+      30             :      *
+      31             :      * Implementation inspired by: https://www.codeproject.com/Articles/190833/Circular-Values-Math-and-Statistics-with-Cplusplus
+      32             :      *
+      33             :      * \parblock
+      34             :      * \note For a better intuitive understanding of the used functions, the term **walk** is sometimes used in the function explanations.
+      35             :      * You can imagine walking along the circle from one angle to another (represented by the circular quantities).
+      36             :      * The walk may be the shortest - then you're walking in such a manner that you reach the other point in the least of steps.
+      37             :      * The walk may also be oriented - then you're walking in a specific direction (ie. according to the increasing/decreasing angle).
+      38             :      * \endparblock
+      39             :      *
+      40             :      * \parblock
+      41             :      * \note The terms **circular quantity** and **value** are used in the function explanations.
+      42             :      * A circular quantity is eg. an angle and the same quantity may be represented using different values: the same angle is represented by the values of 100
+      43             :      * degrees and 460 degrees. \endparblock
+      44             :      *
+      45             :      * \tparam flt floating data type to be used by this class.
+      46             :      */
+      47             :     template <typename flt, class spec>
+      48             :     struct cyclic
+      49             :     {
+      50             :       /*!
+      51             :        * \brief Default constructor.
+      52             :        *
+      53             :        * Sets the value to the minimum.
+      54             :        */
+      55           0 :       cyclic() : val(minimum){};
+      56             :       /*!
+      57             :        * \brief Constructor overload.
+      58             :        *
+      59             :        * \param val initialization value (will be wrapped).
+      60             :        */
+      61     9505861 :       cyclic(const flt val) : val(wrap(val)){};
+      62             :       /*!
+      63             :        * \brief Copy constructor.
+      64             :        *
+      65             :        * \param val initialization value.
+      66             :        */
+      67     2157137 :       cyclic(const cyclic& other) : val(other.val){};
+      68             :       /*!
+      69             :        * \brief Copy constructor.
+      70             :        *
+      71             :        * \param val initialization value.
+      72             :        */
+      73      785709 :       cyclic(const spec& other) : val(other.val){};
+      74             : 
+      75             :       /*!
+      76             :        * \brief Getter for \p val.
+      77             :        *
+      78             :        * \return the value.
+      79             :        */
+      80       50008 :       flt value() const
+      81             :       {
+      82       50008 :         return val;
+      83             :       };
+      84             : 
+      85             :       static constexpr flt minimum = spec::minimum;   /*!< \brief Minimum of the valid interval of wrapped values \f$ m \f$ */
+      86             :       static constexpr flt supremum = spec::supremum; /*!< \brief Supremum of the valid interval of wrapped values \f$ s \f$ */
+      87             :       static constexpr flt range =
+      88             :           supremum - minimum; /*!< \brief Range of the valid interval of wrapped values \f$ r \f$ (also the period of the cyclic quantity). */
+      89             :       static constexpr flt half_range =
+      90             :           range / flt(2); /*!< \brief Half of the range of the valid interval of wrapped values \f$ r/2 \f$ (used for some calculations). */
+      91             : 
+      92             :       /* static_assert((supremum > minimum), "cyclic value: Range not valid"); */
+      93             : 
+      94             :       /*!
+      95             :        * \brief Checks if \p val is within the valid interval of wrapped values.
+      96             :        *
+      97             :        * \param val the value to be checked.
+      98             :        * \returns   true if \p val is within the valid interval of wrapped values.
+      99             :        */
+     100           0 :       static bool inRange(const flt val)
+     101             :       {
+     102           0 :         return val >= minimum && val < supremum;
+     103             :       }
+     104             : 
+     105             :       /*!
+     106             :        * \brief Returns \p val, converted to the valid interval of values.
+     107             :        *
+     108             :        * The wrapped value represents the same quantity as the parameter (ie. \f$ v' = v + kr \f$, where \f$ v \f$ is the parameter and \f$ v' \f$ is the
+     109             :        * returned value).
+     110             :        *
+     111             :        * \param val the value to be wrapped.
+     112             :        * \returns   \p val wrapped to the valid interval of values.
+     113             :        */
+     114    10323581 :       static flt wrap(const flt val)
+     115             :       {
+     116             :         // these few ifs should cover most cases, improving speed and precision
+     117    10323581 :         if (val >= minimum)
+     118             :         {
+     119     9589994 :           if (val < supremum)  // value is actually in range and doesn't need to be wrapped
+     120     9360819 :             return val;
+     121      229179 :           else if (val < supremum + range)
+     122       66581 :             return val - range;  // to avoid unnecessary costly fmod operation for this case (assumed to be significantly more common than the general case)
+     123             :         } else
+     124             :         {
+     125      733705 :           if (val >= minimum - range)
+     126      569783 :             return val + range;  // to avoid unnecessary costly fmod operation for this case (assumed to be significantly more common than the general case)
+     127             :         }
+     128             : 
+     129             :         // general case
+     130      326527 :         const flt rem = std::fmod(val - minimum, range);
+     131      326527 :         const flt wrapped = rem + minimum + std::signbit(rem) * range;
+     132      326342 :         return wrapped;
+     133             :       }
+     134             : 
+     135             :       /*!
+     136             :        * \brief Returns value of the parameter \p what modified so that there is no "jump" between \p from and \t what.
+     137             :        *
+     138             :        * The circular difference between the two input quantities is preserved, but the returned value is modified if necessary so that the linear distance
+     139             :        * between the values is the smallest. This is useful whenever you need to preserve linear continuity of consecutive values, eg. when commanding a
+     140             :        * multi-rotational servo motor or when using a simple linear Kalman filter to estimate circular quantities.
+     141             :        *
+     142             :        * An example of inputs and outputs if \f$ m = 0,~s=360 \f$:
+     143             :        *  \p what | \p from  | \p return
+     144             :        *  ------- | -------- | ---------
+     145             :        *    20    |    10    |   20
+     146             :        *    20    |    350   |   380
+     147             :        *    0     |    350   |   360
+     148             :        *    200   |    10    |  -160
+     149             :        *
+     150             :        * \param what   the value to be unwrapped.
+     151             :        * \param from   the previous value from which the unwrapped value of \p what should have the same circular difference and minimal linear distance.
+     152             :        * \returns      the unwrapped value of \p what.
+     153             :        *
+     154             :        * \warning Note that the returned value may be outside the valid interval of wrapped values, specified by the \p minimum and \p supremum parameters of
+     155             :        * this class.
+     156             :        */
+     157     3124650 :       static flt unwrap(const flt what, const flt from)
+     158             :       {
+     159     3124650 :         return from + diff(what, from);
+     160             :       }
+     161             : 
+     162             :       /*!
+     163             :        * \brief Returns length of the shortest walk in the positive direction from the first parameter to the second one.
+     164             :        *
+     165             :        * \param from   the positive walk starts at this value.
+     166             :        * \param to     the positive walk ends at this value.
+     167             :        * \returns      length of the shortest positive walk from the first parameter to the second one.
+     168             :        *
+     169             :        * \note The returned value is not necessarily the shortest distance between the two circular quantities (see the dist() function for that).
+     170             :        */
+     171           0 :       static flt pdist(const flt from, const flt to)
+     172             :       {
+     173           0 :         return pdist(cyclic(from), cyclic(to));
+     174             :       }
+     175             : 
+     176           0 :       static flt pdist(const cyclic from, const cyclic to)
+     177             :       {
+     178           0 :         const flt tmp = to.val - from.val;
+     179           0 :         const flt dist = tmp + std::signbit(tmp) * range;
+     180           0 :         return dist;
+     181             :       }
+     182             : 
+     183             :       /*!
+     184             :        * \brief Returns the difference between the two circular values.
+     185             :        *
+     186             :        * The difference may also be interpreted as length of the shortest walk between the two values, with a sign according to the direction of the shortest
+     187             :        * walk.
+     188             :        *
+     189             :        * \param minuend      the \p subtrahend will be subtracted from this value.
+     190             :        * \param subtrahend   this value will be subtracted from the \p minuend.
+     191             :        * \returns            the difference of the two circular quantities.
+     192             :        */
+     193     3609028 :       static flt diff(const flt minuend, const flt subtrahend)
+     194             :       {
+     195     3609028 :         return diff(cyclic(minuend), cyclic(subtrahend));
+     196             :       }
+     197             : 
+     198     4688056 :       static flt diff(const cyclic minuend, const cyclic subtrahend)
+     199             :       {
+     200     4688056 :         const flt d = minuend.val - subtrahend.val;
+     201     4688056 :         if (d < -half_range)
+     202       38147 :           return d + range;
+     203     4649909 :         if (d >= half_range)
+     204       25979 :           return d - range;
+     205     4623934 :         return d;
+     206             :       }
+     207             : 
+     208             :       /*!
+     209             :        * \brief Returns the distance between the two circular values.
+     210             :        *
+     211             :        * The distance may also be interpreted as length of the shortest walk between the two values.
+     212             :        *
+     213             :        * \param from  the first circular quantity.
+     214             :        * \param to    the second circular quantity.
+     215             :        * \returns     distance of the two circular quantities.
+     216             :        *
+     217             :        * \note The order of the parameters doesn't matter.
+     218             :        */
+     219       20376 :       static flt dist(const flt from, const flt to)
+     220             :       {
+     221       20376 :         return dist(cyclic(from), cyclic(to));
+     222             :       }
+     223             : 
+     224      413670 :       static flt dist(const cyclic from, const cyclic to)
+     225             :       {
+     226      413670 :         return std::abs(diff(from, to));
+     227             :       }
+     228             : 
+     229             :       /*!
+     230             :        * \brief Interpolation between two circular quantities without wrapping of the result.
+     231             :        *
+     232             :        * This function doesn't wrap the returned value.
+     233             :        *
+     234             :        * \param from  the first circular quantity.
+     235             :        * \param to    the second circular quantity.
+     236             :        * \param coeff the interpolation coefficient.
+     237             :        * \returns     interpolation of the two circular quantities using the coefficient.
+     238             :        *
+     239             :        * \warning Note that the returned value may be outside the valid interval of wrapped values, specified by the \p minimum and \p supremum parameters of
+     240             :        * this class.
+     241             :        */
+     242      655178 :       static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
+     243             :       {
+     244      655178 :         return interpUnwrapped(cyclic(from), cyclic(to), coeff);
+     245             :       }
+     246             : 
+     247      655178 :       static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     248             :       {
+     249      655178 :         const flt dang = diff(to, from);
+     250      655178 :         const flt intp = from.val + coeff * dang;
+     251      655178 :         return intp;
+     252             :       }
+     253             : 
+     254             :       /*!
+     255             :        * \brief Interpolation between two circular quantities.
+     256             :        *
+     257             :        * This function wraps the returned value so that it is in the interval of valid values.
+     258             :        *
+     259             :        * \param from  the first circular quantity.
+     260             :        * \param to    the second circular quantity.
+     261             :        * \param coeff the interpolation coefficient.
+     262             :        * \returns     interpolation of the two circular quantities using the coefficient, wrapped to the interval of valid values.
+     263             :        */
+     264      655178 :       static flt interp(const flt from, const flt to, const flt coeff)
+     265             :       {
+     266      655178 :         return wrap(interpUnwrapped(from, to, coeff));
+     267             :       }
+     268             : 
+     269           0 :       static flt interp(const cyclic from, const cyclic to, const flt coeff)
+     270             :       {
+     271           0 :         return wrap(interpUnwrapped(from, to, coeff));
+     272             :       }
+     273             : 
+     274             :       /*!
+     275             :        * \brief Interpolation between two circular quantities in the positive direction without wrapping of the result.
+     276             :        *
+     277             :        * Interpolates the two values in the positive direction from the first parameter to the second by the coefficient.
+     278             :        * This function doesn't wrap the returned value.
+     279             :        *
+     280             :        * \param from  the first circular quantity.
+     281             :        * \param to    the second circular quantity.
+     282             :        * \param coeff the interpolation coefficient.
+     283             :        * \returns     interpolation of the two circular quantities using the coefficient.
+     284             :        *
+     285             :        * \warning Note that the returned value may be outside the valid interval of wrapped values, specified by the \p minimum and \p supremum parameters of
+     286             :        * this class.
+     287             :        */
+     288           0 :       static flt pinterpUnwrapped(const flt from, const flt to, const flt coeff)
+     289             :       {
+     290           0 :         return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
+     291             :       }
+     292             : 
+     293           0 :       static flt pinterpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     294             :       {
+     295           0 :         const flt dang = pdist(to, from);
+     296           0 :         const flt intp = from.val + coeff * dang;
+     297           0 :         return intp;
+     298             :       }
+     299             : 
+     300             :       /*!
+     301             :        * \brief Interpolation between two circular quantities in the positive direction.
+     302             :        *
+     303             :        * Interpolates the two values in the positive direction from the first parameter to the second by the coefficient.
+     304             :        * This function wraps the returned value so that it is in the interval of valid values.
+     305             :        *
+     306             :        * \param from  the first circular quantity.
+     307             :        * \param to    the second circular quantity.
+     308             :        * \param coeff the interpolation coefficient.
+     309             :        * \returns     interpolation of the two circular quantities using the coefficient, wrapped to the interval of valid values.
+     310             :        */
+     311           0 :       static flt pinterp(const flt from, const flt to, const flt coeff)
+     312             :       {
+     313           0 :         return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
+     314             :       }
+     315             : 
+     316           0 :       static flt pinterp(const cyclic from, const cyclic to, const flt coeff)
+     317             :       {
+     318           0 :         return wrap(pinterpUnwrapped(from, to, coeff));
+     319             :       }
+     320             : 
+     321             :       /*!
+     322             :        * \brief Conversion between two different circular quantities.
+     323             :        *
+     324             :        * This function converts its parameter, interpreted as the circular quantity, represented by this class, to the \p other_t type of circular quantity.
+     325             :        *
+     326             :        * \param what       the circular quantity to be converted.
+     327             :        * \returns          the circular quantity converted to the range of \p other_t.
+     328             :        * \tparam other_t   type of the circular quantity to be converted to.
+     329             :        *
+     330             :        * \warning For the purposes of this function, it is assumed that the range of one type corresponds to the whole range of the other type and zeros of both
+     331             :        * types correspond to each other (such as when converting eg. degrees to radians).
+     332             :        */
+     333             :       template <class other_t>
+     334           1 :       static other_t convert(const cyclic& what)
+     335             :       {
+     336           1 :         return other_t(what.val / range * other_t::range);
+     337             :       }
+     338             : 
+     339             :       /*!
+     340             :        * \brief Conversion between two different circular quantities.
+     341             :        *
+     342             :        * This method returns the circular quantity, represented by this object, converted to the \p other_t type of circular quantity.
+     343             :        *
+     344             :        * \returns          the circular quantity converted to the range of \p other_t.
+     345             :        * \tparam other_t   type of the circular quantity to be converted to.
+     346             :        *
+     347             :        * \warning For the purposes of this function, it is assumed that the range of one type corresponds to the whole range of the other type and zeros of both
+     348             :        * types correspond to each other (such as when converting eg. degrees to radians).
+     349             :        */
+     350             :       template <class other_t>
+     351           1 :       other_t convert() const
+     352             :       {
+     353           1 :         return other_t(val / range * other_t::range);
+     354             :       }
+     355             : 
+     356             :       // | ------------------------ Operators ----------------------- |
+     357             :       /*!
+     358             :        * \brief Assignment operator.
+     359             :        *
+     360             :        * \param nval value to be assigned (will be wrapped).
+     361             :        * \return     reference to self.
+     362             :        */
+     363           0 :       cyclic& operator=(const flt nval)
+     364             :       {
+     365           0 :         val = wrap(nval);
+     366           0 :         return *this;
+     367             :       };
+     368             :       /*!
+     369             :        * \brief Assignment operator.
+     370             :        *
+     371             :        * \param other value to be assigned.
+     372             :        * \return      reference to self.
+     373             :        */
+     374           0 :       cyclic& operator=(const cyclic& other)
+     375             :       {
+     376           0 :         val = other.val;
+     377           0 :         return *this;
+     378             :       };
+     379             :       /*!
+     380             :        * \brief Move operator.
+     381             :        *
+     382             :        * \param other value to be assigned.
+     383             :        * \return      reference to self.
+     384             :        */
+     385           6 :       cyclic& operator=(cyclic&& other)
+     386             :       {
+     387           6 :         val = other.val;
+     388           6 :         return *this;
+     389             :       };
+     390             : 
+     391             :       /*!
+     392             :        * \brief Addition compound operator.
+     393             :        *
+     394             :        * \param other value to be added.
+     395             :        * \return      reference to self.
+     396             :        */
+     397           1 :       cyclic& operator+=(const cyclic& other)
+     398             :       {
+     399           1 :         val = wrap(val + other.val);
+     400           1 :         return *this;
+     401             :       };
+     402             : 
+     403             :       /*!
+     404             :        * \brief Subtraction compound operator.
+     405             :        *
+     406             :        * \param other value to be subtracted.
+     407             :        * \return      reference to self.
+     408             :        */
+     409           1 :       cyclic& operator-=(const cyclic& other)
+     410             :       {
+     411           1 :         val = diff(val, other.val);
+     412           1 :         return *this;
+     413             :       };
+     414             : 
+     415             :       /*!
+     416             :        * \brief Addition operator.
+     417             :        *
+     418             :        * \param lhs left-hand-side.
+     419             :        * \param rhs right-hand-side.
+     420             :        * \return    the result of adding the two angles.
+     421             :        */
+     422       50004 :       friend spec operator+(const cyclic& lhs, const cyclic& rhs)
+     423             :       {
+     424       50004 :         return wrap(lhs.val + rhs.val);
+     425             :       }
+     426             : 
+     427             :       /*!
+     428             :        * \brief Subtraction operator (uses the diff() method).
+     429             :        *
+     430             :        * \param lhs left-hand-side.
+     431             :        * \param rhs right-hand-side.
+     432             :        * \return    the result of subtracting rhs from lhs.
+     433             :        */
+     434       10009 :       friend flt operator-(const cyclic& lhs, const cyclic& rhs)
+     435             :       {
+     436       10009 :         return diff(lhs, rhs);
+     437             :       }
+     438             : 
+     439             :       protected:
+     440             :         flt val;
+     441             :       };
+     442             : 
+     443             :     /*!
+     444             :      * \brief Implementation of the comparison operation between two angles.
+     445             :      *
+     446             :      * An angle is considered to be smaller than another angle if it is shorter - closer to zero.
+     447             :      *
+     448             :      * \param lhs left-hand-side.
+     449             :      * \param rhs right-hand-side.
+     450             :      * \return    true iff the shortest unsigned walk from lhs to 0 is less than from rhs to 0.
+     451             :      */
+     452             :     template <typename flt, class spec>
+     453           4 :     bool operator<(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
+     454             :     {
+     455           4 :       return cyclic<flt, spec>::dist(lhs, 0) < cyclic<flt, spec>::dist(rhs, 0);
+     456             :     }
+     457             : 
+     458             :     /*!
+     459             :      * \brief Implementation of the comparison operation between two angles.
+     460             :      *
+     461             :      * An angle is considered to be larger than another angle if it is longer - further from zero.
+     462             :      *
+     463             :      * \param lhs left-hand-side.
+     464             :      * \param rhs right-hand-side.
+     465             :      * \return    true iff the shortest unsigned walk from lhs to 0 is more than from rhs to 0.
+     466             :      */
+     467             :     template <typename flt, class spec>
+     468           4 :     bool operator>(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
+     469             :     {
+     470           4 :       return cyclic<flt, spec>::dist(lhs, 0) > cyclic<flt, spec>::dist(rhs, 0);
+     471             :     }
+     472             : 
+     473             :     /*!
+     474             :      * \brief Implementation of the stream output operator.
+     475             :      *
+     476             :      * \param out the stream to write the angle to.
+     477             :      * \param ang the angle to be written.
+     478             :      * \return    a reference to the stream.
+     479             :      */
+     480             :     template <typename flt, class spec>
+     481             :     std::ostream& operator<<(std::ostream &out, const cyclic<flt, spec>& ang)
+     482             :     {
+     483             :       return (out << ang.value());
+     484             :     }
+     485             : 
+     486             :     /*!
+     487             :      * \brief Convenience specialization of the cyclic class for unsigned radians (from $0$ to $2\pi$).
+     488             :      */
+     489             :     struct radians : public cyclic<double, radians>
+     490             :     {
+     491             :       using cyclic<double, radians>::cyclic;  // necessary to inherit constructors
+     492             :       static constexpr double minimum = 0;
+     493             :       static constexpr double supremum = 2 * M_PI;
+     494             :     };
+     495             : 
+     496             :     /*!
+     497             :      * \brief Convenience specialization of the cyclic class for signed radians (from $-\pi$ to $\pi$).
+     498             :      */
+     499             :     struct sradians : public cyclic<double, sradians>
+     500             :     {
+     501             :       using cyclic<double, sradians>::cyclic;  // necessary to inherit constructors
+     502             :       static constexpr double minimum = -M_PI;
+     503             :       static constexpr double supremum = M_PI;
+     504             :     };
+     505             : 
+     506             :     /*!
+     507             :      * \brief Convenience specialization of the cyclic class for unsigned degrees (from $0$ to $360$).
+     508             :      */
+     509             :     struct degrees : public cyclic<double, degrees>
+     510             :     {
+     511             :       using cyclic<double, degrees>::cyclic;  // necessary to inherit constructors
+     512             :       static constexpr double minimum = 0;
+     513             :       static constexpr double supremum = 360;
+     514             :     };
+     515             : 
+     516             :     /*!
+     517             :      * \brief Convenience specialization of the cyclic class for signed degrees (from $-180$ to $180$).
+     518             :      */
+     519             :     struct sdegrees : public cyclic<double, sdegrees>
+     520             :     {
+     521             :       using cyclic<double, sdegrees>::cyclic;  // necessary to inherit constructors
+     522             :       static constexpr double minimum = -180;
+     523             :       static constexpr double supremum = 180;
+     524             :     };
+     525             :   }  // namespace geometry
+     526             : }  // namespace mrs_lib
+     527             : 
+     528             : #endif  // CYCLIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html new file mode 100644 index 0000000000..1ad3dfadb9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html @@ -0,0 +1,152 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..966598f52a04aad7097c45178d084b601fee1a23 GIT binary patch literal 2145 zcmV-n2%h(eP)ztac=LGdqZvm__Cy*;dNOmM>B;1gHpm39@cZ%O zo?OEbB%EeIK<43HR=#8OfIF6AR0ZMg;XeTAM1kZCYC>dY$_&f0mBhxF z5qxarj^u6udxMlZGR2bG2SY}qvfda;4-0{@P+@~j6|ifMpP!#!&-HkG;rgsB0NiTt zT0p}!`|BxObM#`*CTrax@8kO8$8lhu>l)T!uHm=F z`2Uyd2GhWG*NRAo0{{ls7GNhr6G;#D&@Y}SlC@y|d-3$x1j-S_eE<%mdj;OTG%L8k zwPqAmlHhYdX_HJVg*9#p*j7_ul7hu+X7*BerKF#1) znkZ)+m*NHUUdB`?wR+H&Ps$r0#H71=?Dts@ZPiOF>vsDYNk})y!q2e#47{&K+_8h5pxxg zM0rEnnBu9rq2n7eQl0#!1%2KcK9#@MhxjYrLq0U9(mrv!z#yvFwSBG*wV1UIG*A#Vyyf+7XT!r zP{65$-0OrCp5W>Pj`J}9_L9!5$7%Uh09-6!5zd%dx-l1IQaVwG8|&ubaDscHyvCUn zygu;tYYLQsu$9j6NkFr0%3Vu( zQyEUga9Mh|?f_4Y;X7uiHq}GsXOx*(1+S)Nu8b9*-3+~bfDv|C?4Z!qPzGc# z%rh%F)G#yO-<**}-Xytmj$dC%?3&!|6zSdWDs)$q+>xJ&#Ln}_N{-C|SNg4s=a}?0 z1HgQqF@_z?1j}iH9mRP40Py07geN6BOr7^as@vafW}iuui@h!VY}oB?(4)T4vGaAd zU5|Q^mb92Or!AZ?r9&i5qTXhOzwBCP<~y=$>I!PNW26@78B(;da_}M7NfFoUdU8Hc`Q0KVm1r&4hm$^0!Q=8o-CdSGaL`RdRdDy}k zd=yj|P-2%i5haNH(f>wo=a#fwURvVbw8<AD5@K3(5x!k^Oht#zIK z$LYG=ufOEimCgJlU9W`#Y6tj(bbS=XCm+dc-ME_owkiNL3Z7i)jT{-^(A5;V3Fpo? z*V6+Cb4J1CH5o?@@DhDtGUCg-kt^w%K9@b1Fn9f$A2DI2k88&2x^Bj>b{12QP=+7+)+@OQzU2!27`rJ?R$Pf` zs9P>$0*A0=t*sa`M&xKQp)h46&TkspY}dZ}bpkuU_{3nbl{>xmI8ifFDhz6dAGZ|L z+2>! + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-25 05:42:57Functions:5112042.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
cyclic.h +
69.0%69.0%
+ 		69.0 %60 / 8741.0 %48 / 117
<unnamed>69.0 %60 / 8741.0 %48 / 117
misc.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
<unnamed>100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html b/mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html new file mode 100644 index 0000000000..badfce1eae --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-25 05:42:57Functions:5112042.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
cyclic.h +
69.0%69.0%
+ 		69.0 %60 / 8741.0 %48 / 117
<unnamed>69.0 %60 / 8741.0 %48 / 117
misc.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
<unnamed>100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/index-detail.html b/mrs_lib/include/mrs_lib/geometry/index-detail.html new file mode 100644 index 0000000000..1a7c7733eb --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-25 05:42:57Functions:5112042.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
cyclic.h +
69.0%69.0%
+ 		69.0 %60 / 8741.0 %48 / 117
<unnamed>69.0 %60 / 8741.0 %48 / 117
misc.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
<unnamed>100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/index-sort-f.html b/mrs_lib/include/mrs_lib/geometry/index-sort-f.html new file mode 100644 index 0000000000..ec448d1ba6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-25 05:42:57Functions:5112042.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
cyclic.h +
69.0%69.0%
+ 		69.0 %60 / 8741.0 %48 / 117
misc.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/index-sort-l.html b/mrs_lib/include/mrs_lib/geometry/index-sort-l.html new file mode 100644 index 0000000000..83fedef278 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-25 05:42:57Functions:5112042.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
cyclic.h +
69.0%69.0%
+ 		69.0 %60 / 8741.0 %48 / 117
misc.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/index.html b/mrs_lib/include/mrs_lib/geometry/index.html new file mode 100644 index 0000000000..169296f1b0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-25 05:42:57Functions:5112042.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
cyclic.h +
69.0%69.0%
+ 		69.0 %60 / 8741.0 %48 / 117
misc.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html b/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html new file mode 100644 index 0000000000..022eeec7c4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
Eigen::Matrix<double, (3)+(1), 1, ((Eigen::StorageOptions)0)|(((((3)+(1))==(1))&&((1)!=(1)))?((Eigen::StorageOptions)1) : ((((1)==(1))&&(((3)+(1))!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), (3)+(1), 1> mrs_lib::geometry::toHomogenous<3>(Eigen::Matrix<double, 3, 1, ((Eigen::StorageOptions)0)|((((3)==(1))&&((1)!=(1)))?((Eigen::StorageOptions)1) : ((((1)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 1> const&)2
double mrs_lib::geometry::headingFromRot<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)6
double mrs_lib::geometry::headingFromRot<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&)112765
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.func.html b/mrs_lib/include/mrs_lib/geometry/misc.h.func.html new file mode 100644 index 0000000000..65fc4c8955 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
Eigen::Matrix<double, (3)+(1), 1, ((Eigen::StorageOptions)0)|(((((3)+(1))==(1))&&((1)!=(1)))?((Eigen::StorageOptions)1) : ((((1)==(1))&&(((3)+(1))!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), (3)+(1), 1> mrs_lib::geometry::toHomogenous<3>(Eigen::Matrix<double, 3, 1, ((Eigen::StorageOptions)0)|((((3)==(1))&&((1)!=(1)))?((Eigen::StorageOptions)1) : ((((1)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 1> const&)2
double mrs_lib::geometry::headingFromRot<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&)112765
double mrs_lib::geometry::headingFromRot<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)6
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.frameset.html new file mode 100644 index 0000000000..609f4cb0f6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html new file mode 100644 index 0000000000..a8edd5d9b9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html @@ -0,0 +1,408 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines useful geometry utilities and functions.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :      \author Petr Štibinger - stibipet@fel.cvut.cz
+       6             :  */
+       7             : 
+       8             : #ifndef GEOMETRY_MISC_H
+       9             : #define GEOMETRY_MISC_H
+      10             : 
+      11             : #include <cmath>
+      12             : #include <Eigen/Dense>
+      13             : #include <geometry_msgs/Point.h>
+      14             : #include <geometry_msgs/Quaternion.h>
+      15             : 
+      16             : namespace mrs_lib
+      17             : {
+      18             :   namespace geometry
+      19             :   {
+      20             :     template <int dims>
+      21             :     using vec_t = Eigen::Matrix<double, dims, 1>;
+      22             : 
+      23             :     using pt2_t = vec_t<2>;
+      24             :     using vec2_t = vec_t<2>;
+      25             :     using pt3_t = vec_t<3>;
+      26             :     using vec3_t = vec_t<3>;
+      27             : 
+      28             :     using mat3_t = Eigen::Matrix3d;
+      29             : 
+      30             :     using quat_t = Eigen::Quaterniond;
+      31             :     using anax_t = Eigen::AngleAxisd;
+      32             : 
+      33             :     template <int dims>
+      34           2 :     vec_t<dims + 1> toHomogenous(const vec_t<dims>& vec)
+      35             :     {
+      36           2 :       const Eigen::Matrix<double, dims + 1, 1> ret((Eigen::Matrix<double, dims + 1, 1>() << vec, 1).finished());
+      37           2 :       return ret;
+      38             :     }
+      39             : 
+      40             :     // | ----------------- Angle-related functions ---------------- |
+      41             : 
+      42             :     /* angle-related functions //{ */
+      43             : 
+      44             :     /* headingFromRot() //{ */
+      45             : 
+      46             :     /*!
+      47             :      * \brief Returns the heading angle from the rotation.
+      48             :      *
+      49             :      * Heading is defined as the angle of a [1,0,0] vector rotated by the rotation and projected to the XY plane from the X axis.
+      50             :      *
+      51             :      * \param rot the rotation to extract the heading angle from.
+      52             :      *
+      53             :      * \returns the heading angle.
+      54             :      *
+      55             :      */
+      56             :     template <typename T>
+      57      112771 :     double headingFromRot(const T& rot)
+      58             :     {
+      59      112771 :       const vec3_t rot_vec = rot*vec3_t::UnitX();
+      60      225543 :       return std::atan2(rot_vec.y(), rot_vec.x());
+      61             :     }
+      62             : 
+      63             :     //}
+      64             : 
+      65             :     /* angleBetween() //{ */
+      66             : 
+      67             :     /*!
+      68             :      * \brief Returns the angle between two vectors, taking orientation into account.
+      69             :      *
+      70             :      * This implementation uses \p atan2 instead of just \p acos and thus it properly
+      71             :      * takes into account orientation of the vectors, returning angle in all four quadrants.
+      72             :      *
+      73             :      * \param a vector from which the angle will be measured.
+      74             :      * \param b vector to which the angle will be measured.
+      75             :      *
+      76             :      * \returns    angle from \p a to \p b.
+      77             :      *
+      78             :      */
+      79             :     double angleBetween(const vec2_t& a, const vec2_t& b);
+      80             : 
+      81             :     /*!
+      82             :      * \brief Returns the angle between two vectors, taking orientation into account.
+      83             :      *
+      84             :      * This implementation uses \p atan2 instead of just \p acos and thus it properly
+      85             :      * takes into account orientation of the vectors, returning angle in all four quadrants.
+      86             :      *
+      87             :      * \param a vector from which the angle will be measured.
+      88             :      * \param b vector to which the angle will be measured.
+      89             :      *
+      90             :      * \returns    angle from \p a to \p b.
+      91             :      *
+      92             :      */
+      93             :     double angleBetween(const vec3_t& a, const vec3_t& b);
+      94             : 
+      95             :     //}
+      96             : 
+      97             :     /* angleaxisBetween() //{ */
+      98             : 
+      99             :     /*!
+     100             :      * \brief Returns the rotation between two vectors, represented as angle-axis.
+     101             :      *
+     102             :      * To avoid singularities, a \p tolerance parameter is used:
+     103             :      * * If the absolute angle between the two vectors is less than \p tolerance, a zero rotation is returned.
+     104             :      * * If the angle between the two vectors is closer to \f$ \pi \f$ than \p tolerance, a \f$ \pi \f$ rotation is returned.
+     105             :      *
+     106             :      * \param a vector from which the rotation starts.
+     107             :      * \param b vector at which the rotation ends.
+     108             :      *
+     109             :      * \returns    rotation from \p a to \p b.
+     110             :      *
+     111             :      */
+     112             :     anax_t angleaxisBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     113             : 
+     114             :     //}
+     115             : 
+     116             :     /* quaternionBetween() //{ */
+     117             : 
+     118             :     /*!
+     119             :      * \brief Returns the rotation between two vectors, represented as a quaternion.
+     120             :      *
+     121             :      * Works the same as the angleaxisBetween() function (in fact it is used in the implementation).
+     122             :      *
+     123             :      * \param a vector from which the rotation starts.
+     124             :      * \param b vector at which the rotation ends.
+     125             :      *
+     126             :      * \returns    rotation from \p a to \p b.
+     127             :      *
+     128             :      */
+     129             :     quat_t quaternionBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     130             : 
+     131             :     //}
+     132             : 
+     133             :     /* rotationBetween() //{ */
+     134             : 
+     135             :     /*!
+     136             :      * \brief Returns the rotation between two vectors, represented as a rotation matrix.
+     137             :      *
+     138             :      * Works the same as the angleaxisBetween() function (in fact it is used in the implementation).
+     139             :      *
+     140             :      * \param a vector from which the rotation starts.
+     141             :      * \param b vector at which the rotation ends.
+     142             :      *
+     143             :      * \returns    rotation from \p a to \p b.
+     144             :      *
+     145             :      */
+     146             :     Eigen::Matrix3d rotationBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     147             : 
+     148             :     //}
+     149             : 
+     150             :     /* haversin() //{ */
+     151             : 
+     152             :     /**
+     153             :      * @brief computes the haversine (half of versine) for a given angle
+     154             :      *
+     155             :      * @param angle angle in radians
+     156             :      *
+     157             :      * @return
+     158             :      */
+     159             :     double haversin(const double angle);
+     160             : 
+     161             :     //}
+     162             : 
+     163             :     /* invHaversin() //{ */
+     164             : 
+     165             :     /**
+     166             :      * @brief computes the inverse haversine angle for a given value
+     167             :      *
+     168             :      * @param value
+     169             :      *
+     170             :      * @return angle in radians
+     171             :      */
+     172             :     double invHaversin(const double value);
+     173             : 
+     174             :     //}
+     175             : 
+     176             :     /* solidAngle //{ */
+     177             : 
+     178             :     /**
+     179             :      * @brief computes the solid angle for a spherical surface corresponding to a 'triangle' with edge lengths a, b, c
+     180             :      *
+     181             :      * @param a
+     182             :      * @param b
+     183             :      * @param c
+     184             :      *
+     185             :      * @return solid angle in steradians
+     186             :      */
+     187             :     double solidAngle(double a, double b, double c);
+     188             : 
+     189             :     //}
+     190             : 
+     191             :     /* quaternionFromEuler() //{ */
+     192             : 
+     193             :     /**
+     194             :      * @brief create a quaternion from 3 provided Euler angles
+     195             :      *
+     196             :      * @param x Euler angle in radians
+     197             :      * @param y Euler angle in radians
+     198             :      * @param z Euler angle in radians
+     199             :      *
+     200             :      * @return quaternion
+     201             :      */
+     202             :     quat_t quaternionFromEuler(double x, double y, double z);
+     203             : 
+     204             :     /**
+     205             :      * @brief create a quaternion from Euler angles provided as a vector
+     206             :      *
+     207             :      * @param euler components of the rotation provided as vector of Euler angles
+     208             :      *
+     209             :      * @return quaternion
+     210             :      */
+     211             :     quat_t quaternionFromEuler(const Eigen::Vector3d& euler);
+     212             : 
+     213             :     //}
+     214             : 
+     215             :     /**
+     216             :      * @brief create a quaternion from heading
+     217             :      *
+     218             :      * Heading is defined as the angle of a [1,0,0] vector rotated by the rotation and projected to the XY plane from the X axis.
+     219             :      *
+     220             :      * @param heading the heading angle.
+     221             :      *
+     222             :      * @return the quaternion corresponding to the heading rotation.
+     223             :      */
+     224             :     quat_t quaternionFromHeading(const double heading);
+     225             : 
+     226             :     //}
+     227             : 
+     228             :     // | ----------------- Miscellaneous functions ---------------- |
+     229             : 
+     230             :     /* 2D cross() //{ */
+     231             : 
+     232             :     /*!
+     233             :      * \brief Implementation of cross product for 2D vectors.
+     234             :      *
+     235             :      * Useful e.g. for finding the sine of an angle between two 2D vectors.
+     236             :      *
+     237             :      * \param a first vector of the cross product.
+     238             :      * \param b second vector of the cross product.
+     239             :      *
+     240             :      * \returns    \f$ a \times b \f$ (sine of the angle from \p a to \p b).
+     241             :      *
+     242             :      */
+     243             :     double cross(const vec2_t& a, const vec2_t b);
+     244             : 
+     245             :     //}
+     246             : 
+     247             :     /* vector distance //{ */
+     248             : 
+     249             :     /**
+     250             :      * @brief distnace between two 2D Eigen vectors
+     251             :      *
+     252             :      * @param a
+     253             :      * @param b
+     254             :      *
+     255             :      * @return Euclidean distance
+     256             :      */
+     257             :     double dist(const vec2_t& a, const vec2_t& b);
+     258             : 
+     259             :     /**
+     260             :      * @brief distnace between two 3D Eigen vectors
+     261             :      *
+     262             :      * @param a
+     263             :      * @param b
+     264             :      *
+     265             :      * @return Euclidean distance
+     266             :      */
+     267             :     double dist(const vec3_t& a, const vec3_t& b);
+     268             : 
+     269             :     //}
+     270             : 
+     271             :     /* triangleArea() //{ */
+     272             : 
+     273             :     /**
+     274             :      * @brief uses Heron's formula to compute area of a given triangle using side lengths
+     275             :      *
+     276             :      * @param a length of side1
+     277             :      * @param b length of side2
+     278             :      * @param c length of side3
+     279             :      *
+     280             :      * @return area in units squared
+     281             :      */
+     282             :     double triangleArea(const double a, const double b, const double c);
+     283             : 
+     284             :     //}
+     285             : 
+     286             :     /* sphericalTriangleArea //{ */
+     287             : 
+     288             :     /**
+     289             :      * @brief compute the area of a 'triangle' drawn on a spherical surface
+     290             :      *
+     291             :      * @param a length of side1
+     292             :      * @param b length of side2
+     293             :      * @param c length of side3
+     294             :      *
+     295             :      * @return area in units squared
+     296             :      */
+     297             :     double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c);
+     298             : 
+     299             :     //}
+     300             : 
+     301             :     /* rotateCovariance() //{ */
+     302             :     
+     303             :     /*!
+     304             :      * \brief Returns the covariance rotated using the specified rotation.
+     305             :      *
+     306             :      * \param cov the covariance to be rotated.
+     307             :      * \param rot the rotation use.
+     308             :      *
+     309             :      * \returns a new matrix object containing the rotated covariance.
+     310             :      *
+     311             :      */
+     312             :     template <typename T>
+     313             :     mat3_t rotateCovariance(const mat3_t& cov, const T& rot)
+     314             :     {
+     315             :       const mat3_t matrot(rot);
+     316             :       return matrot*cov*matrot.transpose();
+     317             :     }
+     318             :     
+     319             :     //}
+     320             : 
+     321             :   }  // namespace geometry
+     322             : }  // namespace mrs_lib
+     323             : 
+     324             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html new file mode 100644 index 0000000000..5f250b8ef8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html @@ -0,0 +1,101 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.png b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..581208ad4f1fcef2a803b9ae709887799e133c02 GIT binary patch literal 1071 zcmV+~1kn45P);0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp|&|^9W#|%90LIw8GE%5p? zC5q1}kKGVco(9NAnPql%OSW8V7PG?5hPsSrLPJS{UuHD7nsH1*G>pfNw!rJQjiR{U zpRe|K{5-im^5^pVy=n{a=Wb~W@SgAH_EXp&&*%DD0WR0ic#FdTV?{RXIm(iyo|^*G_>AUPwMkgNID_6 zGgfN~&;?+<64wd{YLyL3Hq@pG$bL#UBHfa~ zJ|8!!u)mVtrb23-f`J~>B^63my;5Pf%jc#-n+z!Bq19|=oR|t@>iVIn0Pa-KA$>In z@l~W6^efzMTMKt3SXMCpJ;EKN+plmxLbwvS<5AaYHZx8V?zp8{K)9?j;4%|ZQ6RhI z+^69BxD#5RdQ3q7XvP~dR$mSor;TQ##P@E0-`tvdKg8mJ>Jk1$HTeRn*+wr z9VtW(NhK1Sklq6{V1$vMx6C0&z>v&PJ}z|V)Fr^6@y9JQAf+eTrB3Y@wY%;)oslL0 zJw8%yp8$geyROz#^a%7q7wUFhIR;^%)wI?WNzP8Fyym9@e|bGO;2?pI&Lb1@`brmG zEZDi)<`vDL12JBqw8KN(N1kIPZA2;xjEX|?Al$Vx2K&0~kuTA2kHXr$Ynu!E p@Zka + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/gps_conversions.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - gps_conversions.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10413676.5 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)2006
mrs_lib::UTM(double, double, double*, double*)6017
mrs_lib::LLtoUTM(double, double, double&, double&, char*)142159
mrs_lib::UTMLetterDesignator(double)142300
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/gps_conversions.h.func.html b/mrs_lib/include/mrs_lib/gps_conversions.h.func.html new file mode 100644 index 0000000000..88af425df6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/gps_conversions.h.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/gps_conversions.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - gps_conversions.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10413676.5 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMLetterDesignator(double)142300
mrs_lib::UTM(double, double, double*, double*)6017
mrs_lib::LLtoUTM(double, double, double&, double&, char*)142159
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)2006
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.frameset.html new file mode 100644 index 0000000000..f0ef2f614d --- /dev/null +++ b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/gps_conversions.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html new file mode 100644 index 0000000000..f620cbc9bd --- /dev/null +++ b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html @@ -0,0 +1,387 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/gps_conversions.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - gps_conversions.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10413676.5 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* Taken from utexas-art-ros-pkg:art_vehicle/applanix */
+       2             : 
+       3             : /*
+       4             :  * Conversions between coordinate systems.
+       5             :  *
+       6             :  * Includes LatLong<->UTM.
+       7             :  */
+       8             : 
+       9             : #ifndef _UTM_H
+      10             : #define _UTM_H
+      11             : 
+      12             : /**  @file
+      13             :      @brief Universal Transverse Mercator transforms.
+      14             :      Functions to convert (spherical) latitude and longitude to and
+      15             :      from (Euclidean) UTM coordinates.
+      16             :      @author Chuck Gantz- chuck.gantz@globalstar.com
+      17             :  */
+      18             : 
+      19             : #include <cmath>
+      20             : #include <cstdio>
+      21             : #include <cstdlib>
+      22             : #include <string>
+      23             : 
+      24             : namespace mrs_lib
+      25             : {
+      26             : 
+      27             :   const double RADIANS_PER_DEGREE = M_PI / 180.0;
+      28             :   const double DEGREES_PER_RADIAN = 180.0 / M_PI;
+      29             : 
+      30             :   // WGS84 Parameters
+      31             :   const double WGS84_A  = 6378137.0;         // major axis
+      32             :   const double WGS84_B  = 6356752.31424518;  // minor axis
+      33             :   const double WGS84_F  = 0.0033528107;      // ellipsoid flattening
+      34             :   const double WGS84_E  = 0.0818191908;      // first eccentricity
+      35             :   const double WGS84_EP = 0.0820944379;      // second eccentricity
+      36             : 
+      37             :   // UTM Parameters
+      38             :   const double UTM_K0   = 0.9996;                   // scale factor
+      39             :   const double UTM_FE   = 500000.0;                 // false easting
+      40             :   const double UTM_FN_N = 0.0;                      // false northing on north hemisphere
+      41             :   const double UTM_FN_S = 10000000.0;               // false northing on south hemisphere
+      42             :   const double UTM_E2   = (WGS84_E * WGS84_E);      // e^2
+      43             :   const double UTM_E4   = (UTM_E2 * UTM_E2);        // e^4
+      44             :   const double UTM_E6   = (UTM_E4 * UTM_E2);        // e^6
+      45             :   const double UTM_EP2  = (UTM_E2 / (1 - UTM_E2));  // e'^2
+      46             : 
+      47             :   /**
+      48             :    * Utility function to convert geodetic to UTM position
+      49             :    *
+      50             :    * Units in are floating point degrees (sign for east/west)
+      51             :    *
+      52             :    * Units out are meters
+      53             :    */
+      54        6017 :   static inline void UTM(double lat, double lon, double* x, double* y) {
+      55             :     // constants
+      56             :     const static double m0 = (1 - UTM_E2 / 4 - 3 * UTM_E4 / 64 - 5 * UTM_E6 / 256);
+      57             :     const static double m1 = -(3 * UTM_E2 / 8 + 3 * UTM_E4 / 32 + 45 * UTM_E6 / 1024);
+      58             :     const static double m2 = (15 * UTM_E4 / 256 + 45 * UTM_E6 / 1024);
+      59             :     const static double m3 = -(35 * UTM_E6 / 3072);
+      60             : 
+      61             :     // compute the central meridian
+      62        6017 :     int cm = ((lon >= 0.0) ? ((int)lon - ((int)lon) % 6 + 3) : ((int)lon - ((int)lon) % 6 - 3));
+      63             : 
+      64             :     // convert degrees into radians
+      65        6017 :     double rlat  = lat * RADIANS_PER_DEGREE;
+      66        6017 :     double rlon  = lon * RADIANS_PER_DEGREE;
+      67        6017 :     double rlon0 = cm * RADIANS_PER_DEGREE;
+      68             : 
+      69             :     // compute trigonometric functions
+      70        6017 :     double slat = sin(rlat);
+      71        6017 :     double clat = cos(rlat);
+      72        6017 :     double tlat = tan(rlat);
+      73             : 
+      74             :     // decide the false northing at origin
+      75        6017 :     double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
+      76             : 
+      77        6017 :     double T = tlat * tlat;
+      78        6017 :     double C = UTM_EP2 * clat * clat;
+      79        6017 :     double A = (rlon - rlon0) * clat;
+      80        6017 :     double M = WGS84_A * (m0 * rlat + m1 * sin(2 * rlat) + m2 * sin(4 * rlat) + m3 * sin(6 * rlat));
+      81        6017 :     double V = WGS84_A / sqrt(1 - UTM_E2 * slat * slat);
+      82             : 
+      83             :     // compute the easting-northing coordinates
+      84        6017 :     *x = UTM_FE + UTM_K0 * V * (A + (1 - T + C) * pow(A, 3) / 6 + (5 - 18 * T + T * T + 72 * C - 58 * UTM_EP2) * pow(A, 5) / 120);
+      85        5993 :     *y = fn +
+      86        5993 :          UTM_K0 *
+      87        6010 :              (M + V * tlat * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * pow(A, 4) / 24 + ((61 - 58 * T + T * T + 600 * C - 330 * UTM_EP2) * pow(A, 6) / 720)));
+      88             : 
+      89        5993 :     return;
+      90             :   }
+      91             : 
+      92             : 
+      93             :   /**
+      94             :    * Determine the correct UTM letter designator for the
+      95             :    * given latitude
+      96             :    *
+      97             :    * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S
+      98             :    *
+      99             :    * Written by Chuck Gantz- chuck.gantz@globalstar.com
+     100             :    */
+     101      142300 :   static inline char UTMLetterDesignator(double Lat) {
+     102             :     char LetterDesignator;
+     103             : 
+     104      142300 :     if ((84 >= Lat) && (Lat >= 72))
+     105           0 :       LetterDesignator = 'X';
+     106      142300 :     else if ((72 > Lat) && (Lat >= 64))
+     107           0 :       LetterDesignator = 'W';
+     108      142300 :     else if ((64 > Lat) && (Lat >= 56))
+     109           0 :       LetterDesignator = 'V';
+     110      142300 :     else if ((56 > Lat) && (Lat >= 48))
+     111           0 :       LetterDesignator = 'U';
+     112      142300 :     else if ((48 > Lat) && (Lat >= 40))
+     113      141414 :       LetterDesignator = 'T';
+     114         886 :     else if ((40 > Lat) && (Lat >= 32))
+     115           0 :       LetterDesignator = 'S';
+     116         886 :     else if ((32 > Lat) && (Lat >= 24))
+     117           0 :       LetterDesignator = 'R';
+     118         886 :     else if ((24 > Lat) && (Lat >= 16))
+     119           0 :       LetterDesignator = 'Q';
+     120         886 :     else if ((16 > Lat) && (Lat >= 8))
+     121           0 :       LetterDesignator = 'P';
+     122         886 :     else if ((8 > Lat) && (Lat >= 0))
+     123           2 :       LetterDesignator = 'N';
+     124         884 :     else if ((0 > Lat) && (Lat >= -8))
+     125           0 :       LetterDesignator = 'M';
+     126         884 :     else if ((-8 > Lat) && (Lat >= -16))
+     127           0 :       LetterDesignator = 'L';
+     128         884 :     else if ((-16 > Lat) && (Lat >= -24))
+     129           0 :       LetterDesignator = 'K';
+     130         884 :     else if ((-24 > Lat) && (Lat >= -32))
+     131           0 :       LetterDesignator = 'J';
+     132         884 :     else if ((-32 > Lat) && (Lat >= -40))
+     133           0 :       LetterDesignator = 'H';
+     134         884 :     else if ((-40 > Lat) && (Lat >= -48))
+     135           0 :       LetterDesignator = 'G';
+     136         884 :     else if ((-48 > Lat) && (Lat >= -56))
+     137           0 :       LetterDesignator = 'F';
+     138         884 :     else if ((-56 > Lat) && (Lat >= -64))
+     139           0 :       LetterDesignator = 'E';
+     140         884 :     else if ((-64 > Lat) && (Lat >= -72))
+     141           0 :       LetterDesignator = 'D';
+     142         884 :     else if ((-72 > Lat) && (Lat >= -80))
+     143           0 :       LetterDesignator = 'C';
+     144             :     // 'Z' is an error flag, the Latitude is outside the UTM limits
+     145             :     else
+     146         884 :       LetterDesignator = 'Z';
+     147      142300 :     return LetterDesignator;
+     148             :   }
+     149             : 
+     150             :   /**
+     151             :    * Convert lat/long to UTM coords.  Equations from USGS Bulletin 1532
+     152             :    *
+     153             :    * East Longitudes are positive, West longitudes are negative.
+     154             :    * North latitudes are positive, South latitudes are negative
+     155             :    * Lat and Long are in fractional degrees
+     156             :    *
+     157             :    * Written by Chuck Gantz- chuck.gantz@globalstar.com
+     158             :    */
+     159      142159 :   static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, char* UTMZone) {
+     160      142159 :     double a          = WGS84_A;
+     161      142159 :     double eccSquared = UTM_E2;
+     162      142159 :     double k0         = UTM_K0;
+     163             : 
+     164             :     double LongOrigin;
+     165             :     double eccPrimeSquared;
+     166             :     double N, T, C, A, M;
+     167             : 
+     168             :     // Make sure the longitude is between -180.00 .. 179.9
+     169      142159 :     double LongTemp = (Long + 180) - int((Long + 180) / 360) * 360 - 180;
+     170             : 
+     171      142159 :     double LatRad  = Lat * RADIANS_PER_DEGREE;
+     172      142159 :     double LongRad = LongTemp * RADIANS_PER_DEGREE;
+     173             :     double LongOriginRad;
+     174             :     int    ZoneNumber;
+     175             : 
+     176      142159 :     ZoneNumber = int((LongTemp + 180) / 6) + 1;
+     177             :     // range clamping to shut up some compiler warnings
+     178             :     // (the UTM Zone number should in reality be in the range <1, 60>)
+     179      142159 :     if (ZoneNumber > 99)
+     180           0 :       ZoneNumber = 99;
+     181      142159 :     if (ZoneNumber < -9)
+     182           0 :       ZoneNumber = -9;
+     183             : 
+     184      142159 :     if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
+     185           0 :       ZoneNumber = 32;
+     186             : 
+     187             :     // Special zones for Svalbard
+     188      142159 :     if (Lat >= 72.0 && Lat < 84.0) {
+     189           0 :       if (LongTemp >= 0.0 && LongTemp < 9.0)
+     190           0 :         ZoneNumber = 31;
+     191           0 :       else if (LongTemp >= 9.0 && LongTemp < 21.0)
+     192           0 :         ZoneNumber = 33;
+     193           0 :       else if (LongTemp >= 21.0 && LongTemp < 33.0)
+     194           0 :         ZoneNumber = 35;
+     195           0 :       else if (LongTemp >= 33.0 && LongTemp < 42.0)
+     196           0 :         ZoneNumber = 37;
+     197             :     }
+     198             :     // +3 puts origin in middle of zone
+     199      142159 :     LongOrigin    = (ZoneNumber - 1) * 6 - 180 + 3;
+     200      142159 :     LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
+     201             : 
+     202             :     // compute the UTM Zone from the latitude and longitude
+     203      142159 :     snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+     204             : 
+     205      141848 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     206             : 
+     207      141848 :     N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
+     208      141848 :     T = tan(LatRad) * tan(LatRad);
+     209      141848 :     C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+     210      141848 :     A = cos(LatRad) * (LongRad - LongOriginRad);
+     211             : 
+     212      141848 :     M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad -
+     213      141848 :              (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad) +
+     214      141848 :              (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad) -
+     215      141848 :              (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
+     216             : 
+     217      141848 :     UTMEasting =
+     218      141848 :         (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120) + 500000.0);
+     219             : 
+     220      141848 :     UTMNorthing = (double)(k0 * (M + N * tan(LatRad) *
+     221      141848 :                                          (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
+     222      141848 :                                           (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
+     223      141848 :     if (Lat < 0)
+     224           0 :       UTMNorthing += 10000000.0;  // 10000000 meter offset for southern hemisphere
+     225      141848 :   }
+     226             : 
+     227             :   static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, std::string& UTMZone) {
+     228             :     char zone_buf[] = {0, 0, 0, 0};
+     229             : 
+     230             :     LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, zone_buf);
+     231             : 
+     232             :     UTMZone = zone_buf;
+     233             :   }
+     234             : 
+     235             : 
+     236             :   /**
+     237             :    * Converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
+     238             :    *
+     239             :    * East Longitudes are positive, West longitudes are negative.
+     240             :    * North latitudes are positive, South latitudes are negative
+     241             :    * Lat and Long are in fractional degrees.
+     242             :    *
+     243             :    * Written by Chuck Gantz- chuck.gantz@globalstar.com
+     244             :    */
+     245        2006 :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char* UTMZone, double& Lat, double& Long) {
+     246        2006 :     double                  k0         = UTM_K0;
+     247        2006 :     double                  a          = WGS84_A;
+     248        2006 :     double                  eccSquared = UTM_E2;
+     249             :     double                  eccPrimeSquared;
+     250        2006 :     double                  e1 = (1 - sqrt(1 - eccSquared)) / (1 + sqrt(1 - eccSquared));
+     251             :     double                  N1, T1, C1, R1, D, M;
+     252             :     double                  LongOrigin;
+     253             :     double                  mu, phi1Rad;
+     254             :     [[maybe_unused]] double phi1;
+     255             :     double                  x, y;
+     256             :     int                     ZoneNumber;
+     257             :     char*                   ZoneLetter;
+     258             :     [[maybe_unused]] int    NorthernHemisphere;  // 1 for northern hemispher, 0 for southern
+     259             : 
+     260        2006 :     x = UTMEasting - 500000.0;  // remove 500,000 meter offset for longitude
+     261        2006 :     y = UTMNorthing;
+     262             : 
+     263        2006 :     ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+     264        2006 :     if ((*ZoneLetter - 'N') >= 0)
+     265        2006 :       NorthernHemisphere = 1;  // point is in northern hemisphere
+     266             :     else {
+     267           0 :       NorthernHemisphere = 0;  // point is in southern hemisphere
+     268           0 :       y -= 10000000.0;         // remove 10,000,000 meter offset used for southern hemisphere
+     269             :     }
+     270             : 
+     271        2006 :     LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3;  //+3 puts origin in middle of zone
+     272             : 
+     273        2006 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     274             : 
+     275        2006 :     M  = y / k0;
+     276        2006 :     mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
+     277             : 
+     278        2006 :     phi1Rad = mu + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu) + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu) +
+     279        2006 :               (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
+     280        2006 :     phi1 = phi1Rad * DEGREES_PER_RADIAN;
+     281             : 
+     282        2006 :     N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
+     283        2006 :     T1 = tan(phi1Rad) * tan(phi1Rad);
+     284        2006 :     C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+     285        2006 :     R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
+     286        2006 :     D  = x / (N1 * k0);
+     287             : 
+     288        2006 :     Lat = phi1Rad - (N1 * tan(phi1Rad) / R1) * (D * D / 2 - (5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * eccPrimeSquared) * D * D * D * D / 24 +
+     289        2006 :                                                 (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
+     290        2006 :     Lat = Lat * DEGREES_PER_RADIAN;
+     291             : 
+     292        2006 :     Long = (D - (1 + 2 * T1 + C1) * D * D * D / 6 + (5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * eccPrimeSquared + 24 * T1 * T1) * D * D * D * D * D / 120) /
+     293        2006 :            cos(phi1Rad);
+     294        2006 :     Long = LongOrigin + Long * DEGREES_PER_RADIAN;
+     295        2006 :   }
+     296             : 
+     297             :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, std::string UTMZone, double& Lat, double& Long) {
+     298             :     UTMtoLL(UTMNorthing, UTMEasting, UTMZone.c_str(), Lat, Long);
+     299             :   }
+     300             : 
+     301             : }  // namespace mrs_lib
+     302             : 
+     303             : #endif  // _UTM_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html new file mode 100644 index 0000000000..68d9ba32f7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html @@ -0,0 +1,96 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/gps_conversions.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.png b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3b04ca2bf9dcbddc89b129fb7f6bb4873f2132a0 GIT binary patch literal 1579 zcmV+`2Gse9P)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp>!TNJ4nq%^v^ler$H% z-fGj283TL_eNc!daET)KWeBvhBxf5jz7ROi+L~EQtw8IXG#cq#z{Pf1QwoNjE3Q;L_XG4ZzCaS$Wkr3rWB)BwWg1J;Ip z#I%f?mEo1fv8}9i04CydK#LT4AfE#3iIIV}qG;NaaYj8;+R3c#R*oW`njN6}!IZP9 z>`W;WIhb_O=(Rlq5^KX#Eq*ubjKq)6fja|m_Ad$fCGrewUdE@p9+YICmBLTakAINEX-m5I3bdg$}B-DI)NSFHk}aY2MW|O z0>v3%gaQ-dLfpBb6pOY#7cl}SKmhzgTZ^vZNM#LT>I8s7+=S0wU6a}(xRVS>(+zQJmQfThl2P}%NphW{7P8f){}lgI{0WK?_jo;?@qqvF z@Pa$Ij%RvyORKoa$h9zCXQJjeGO72<4^RN1OxJJ$fhetHDLKiKWu)AetW5#bx3&Np zAY=1>L9>5#LN_#tS69Q~w4#w&JLcamY80MgK*mIVdY~;+i zAH2XmNI{i2=!j(Ptl3&doU6RtBHI`$T}ZW-vl8OUCz>!;o-hbhlX`waFdBMpCcY-NRE zjkQo@ADT$pP>lm)cOV>fH`b!IGz-MBP%{|2$6-jz*W_Kgoo4 z-Vi>K?h7we%(k^voXy!5ycx8!pH}ZS{9&uO3*P$rfmIw`$qj8b@<+8KJProU zng?MTV^FcEI}xS)n^ zXQyICM$GTZC6Z+EC9wFd|E_{aH+n)ToH1<~#Dv2_^J=fLp6Jy;E8iS|Q + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-25 05:42:57Functions:828139459.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12047.2 %493 / 1044
<unnamed>81.7 %98 / 12047.2 %493 / 1044
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6487.5 %42 / 48
<unnamed>85.9 %55 / 6487.5 %42 / 48
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.6 %201 / 206
<unnamed>81.5 %44 / 5497.6 %201 / 206
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
<unnamed>64.3 %9 / 14100.0 %16 / 16
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
<unnamed>100.0 %18 / 18100.0 %18 / 18
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %47 / 47
<unnamed>92.9 %52 / 56100.0 %47 / 47
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html b/mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html new file mode 100644 index 0000000000..86273f7058 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html @@ -0,0 +1,236 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-25 05:42:57Functions:828139459.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
<unnamed>64.3 %9 / 14100.0 %16 / 16
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.6 %201 / 206
<unnamed>81.5 %44 / 5497.6 %201 / 206
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12047.2 %493 / 1044
<unnamed>81.7 %98 / 12047.2 %493 / 1044
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6487.5 %42 / 48
<unnamed>85.9 %55 / 6487.5 %42 / 48
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %47 / 47
<unnamed>92.9 %52 / 56100.0 %47 / 47
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
<unnamed>100.0 %18 / 18100.0 %18 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/index-detail.html b/mrs_lib/include/mrs_lib/impl/index-detail.html new file mode 100644 index 0000000000..ccebb0318b --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail.html @@ -0,0 +1,236 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-25 05:42:57Functions:828139459.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
<unnamed>64.3 %9 / 14100.0 %16 / 16
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.6 %201 / 206
<unnamed>81.5 %44 / 5497.6 %201 / 206
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %47 / 47
<unnamed>92.9 %52 / 56100.0 %47 / 47
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12047.2 %493 / 1044
<unnamed>81.7 %98 / 12047.2 %493 / 1044
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6487.5 %42 / 48
<unnamed>85.9 %55 / 6487.5 %42 / 48
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
<unnamed>100.0 %18 / 18100.0 %18 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/index-sort-f.html b/mrs_lib/include/mrs_lib/impl/index-sort-f.html new file mode 100644 index 0000000000..ba262a2367 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-f.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-25 05:42:57Functions:828139459.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12047.2 %493 / 1044
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6487.5 %42 / 48
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.6 %201 / 206
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %47 / 47
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/index-sort-l.html b/mrs_lib/include/mrs_lib/impl/index-sort-l.html new file mode 100644 index 0000000000..add5849111 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-l.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-25 05:42:57Functions:828139459.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.6 %201 / 206
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12047.2 %493 / 1044
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6487.5 %42 / 48
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %47 / 47
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/index.html b/mrs_lib/include/mrs_lib/impl/index.html new file mode 100644 index 0000000000..ac292aca9c --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-25 05:42:57Functions:828139459.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.6 %201 / 206
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %47 / 47
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12047.2 %493 / 1044
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6487.5 %42 / 48
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html new file mode 100644 index 0000000000..2aac9553ce --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-04-25 05:42:57Functions:1616100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
bool mrs_lib::ParamProvider::getParamImpl<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParam<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParamImpl<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParam<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParamImpl<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&) const1682
bool mrs_lib::ParamProvider::getParam<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&) const1682
bool mrs_lib::ParamProvider::getParamImpl<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&) const3296
bool mrs_lib::ParamProvider::getParam<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&) const3296
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4760
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4760
bool mrs_lib::ParamProvider::getParamImpl<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&) const15907
bool mrs_lib::ParamProvider::getParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&) const15907
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const18818
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const18818
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const38479
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const38479
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html new file mode 100644 index 0000000000..7764f7ca9d --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-04-25 05:42:57Functions:1616100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
bool mrs_lib::ParamProvider::getParamImpl<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&) const15907
bool mrs_lib::ParamProvider::getParamImpl<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&) const3296
bool mrs_lib::ParamProvider::getParamImpl<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&) const1682
bool mrs_lib::ParamProvider::getParamImpl<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParamImpl<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const18818
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const38479
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4760
bool mrs_lib::ParamProvider::getParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&) const15907
bool mrs_lib::ParamProvider::getParam<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&) const3296
bool mrs_lib::ParamProvider::getParam<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&) const1682
bool mrs_lib::ParamProvider::getParam<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParam<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const18818
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const38479
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4760
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.frameset.html new file mode 100644 index 0000000000..150116899e --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html new file mode 100644 index 0000000000..daae4d3437 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-04-25 05:42:57Functions:1616100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef PARAM_PROVIDER_HPP
+       2             : #define PARAM_PROVIDER_HPP
+       3             : 
+       4             : #include <mrs_lib/param_provider.h>
+       5             : 
+       6             : namespace mrs_lib
+       7             : {
+       8             :   template <typename T>
+       9       82957 :   bool ParamProvider::getParam(const std::string& param_name, T& value_out) const
+      10             :   {
+      11             :     try
+      12             :     {
+      13       82957 :       return getParamImpl(param_name, value_out);
+      14             :     }
+      15           0 :     catch (const YAML::Exception& e)
+      16             :     {
+      17           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: YAML-CPP threw an unknown exception: " << e.what());
+      18           0 :       return false;
+      19             :     }
+      20             :   }
+      21             : 
+      22             :   template <typename T>
+      23       82957 :   bool ParamProvider::getParamImpl(const std::string& param_name, T& value_out) const
+      24             :   {
+      25             :     {
+      26       82957 :       const auto found_node = findYamlNode(param_name);
+      27       82957 :       if (found_node.has_value())
+      28             :       {
+      29             :         try
+      30             :         {
+      31             :           // try catch is the only type-generic option...
+      32       71320 :           value_out = found_node.value().as<T>();
+      33       71320 :           return true;
+      34             :         }
+      35           0 :         catch (const YAML::BadConversion& e)
+      36             :         {}
+      37             :       }
+      38             : 
+      39             :     }
+      40             : 
+      41       11637 :     if (m_use_rosparam)
+      42       11637 :       return m_nh.getParam(param_name, value_out);
+      43             : 
+      44           0 :     return false;
+      45             :   }
+      46             : }
+      47             : 
+      48             : #endif // PARAM_PROVIDER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html new file mode 100644 index 0000000000..69615a5af4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html @@ -0,0 +1,32 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ade5b52da3b6b1fdc1937ea78b6f4e330848bd5b GIT binary patch literal 347 zcmeAS@N?(olHy`uVBq!ia0vp^0YGfP!VDx!PCqyTq$C1-LR|m<|Gx?dmcNgUef6J# zVHHpuOr7)IycEdhEbxddW?exI8~cZ8YuYE)5S5wqIYZPZNU}=o)*3ZtZx{i z8u&c^ua)V4vEbQ`uWa8BH!8ks+tGO{D*FweDWeR>Vxdgy18WjYI0V;~sH}Yceuu!e z0;l^`b$NRN&slSXO8W};Xs>xVM=ia4YFWZ0Ew@EJE}d3Ju7autr`5k7@L&&0yLLsS zDBjGHPiM9KksV){!x#LxZ@Z~}-j3&;nl`5^?b8Y_Wi{U3e%V`LO6vLxN&VqovDxc< zWm#+Y&A45X$$2MVXKT~#-D!1_>~XuLzF#sey8p%D#18$4a?^(i^D?KlC?1vmcshZ# noy}F#!c60lb+eQ5{_FfwLML{x?EUc*=xGK|S3j3^P6 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - publisher_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-25 05:42:57Functions:20120697.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > > const&)1
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > > const&)2
mrs_lib::PublisherHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)31
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)89
mrs_lib::PublisherHandler_impl<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)89
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)95
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)95
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)99
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)99
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)99
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)189
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)194
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)194
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)194
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)271
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)271
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)271
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)370
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)370
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)370
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)376
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)376
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)383
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)437
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)437
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)459
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)459
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)543
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)543
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)543
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)564
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)564
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)564
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)708
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)708
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)708
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1685
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1685
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2055
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2055
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2061
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2061
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2134
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2134
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7069
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7069
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9790
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9790
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)10674
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)10674
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)11776
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)11776
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)14546
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)14546
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)17196
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)17196
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)17256
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)17256
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)32672
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)32672
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)86036
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)86036
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)100274
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)100274
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)136855
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)136855
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)139491
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)154965
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)154965
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)190603
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)190605
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)228120
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)228121
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)343229
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)343236
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)344388
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)344411
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)469156
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)469772
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)660441
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)660465
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)734464
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)734474
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1331317
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1331445
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html new file mode 100644 index 0000000000..9adc274d1c --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html @@ -0,0 +1,904 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - publisher_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-25 05:42:57Functions:20120697.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)154965
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)376
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1685
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)190605
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)99
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)99
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)32672
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)564
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)564
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)10674
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)86036
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)344411
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)543
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)543
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)100274
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)660465
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)370
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)370
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7069
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > > const&)2
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)437
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > > const&)1
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2061
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)14546
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1331445
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)708
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)708
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)469772
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)271
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)271
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2134
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)95
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)189
mrs_lib::PublisherHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)136855
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)17196
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2055
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)89
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)17256
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)188
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)343236
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)194
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)194
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)734474
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)139491
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)459
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)383
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9790
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)31
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)11776
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)94
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)228121
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)376
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)154965
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1685
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)190603
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)99
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)32672
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)564
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)10674
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)86036
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)344388
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)543
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)100274
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)660441
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)370
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7069
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)437
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2061
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)14546
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1331317
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)708
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)469156
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)271
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2134
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)95
mrs_lib::PublisherHandler_impl<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)136855
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)17196
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2055
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)89
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)17256
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1684
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)343229
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)194
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)734464
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)459
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9790
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)11776
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)21949
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)94
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)228120
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)376
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.frameset.html new file mode 100644 index 0000000000..0082e6a657 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html new file mode 100644 index 0000000000..aebb751a63 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html @@ -0,0 +1,332 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - publisher_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-25 05:42:57Functions:20120697.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef PUBLISHER_HANDLER_HPP
+       2             : #define PUBLISHER_HANDLER_HPP
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             : 
+       7             : // --------------------------------------------------------------
+       8             : // |                    PublisherHandler_impl                   |
+       9             : // --------------------------------------------------------------
+      10             : 
+      11             : /* PublisherHandler_impl(void) //{ */
+      12             : 
+      13             : template <class TopicType>
+      14             : PublisherHandler_impl<TopicType>::PublisherHandler_impl(void) : publisher_initialized_(false) {
+      15             : }
+      16             : 
+      17             : //}
+      18             : 
+      19             : /* PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int &buffer_size, const bool &latch) //{ */
+      20             : 
+      21             : template <class TopicType>
+      22        6347 : PublisherHandler_impl<TopicType>::PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+      23        6347 :                                                         const double& rate) {
+      24             : 
+      25             :   {
+      26        6347 :     std::scoped_lock lock(mutex_publisher_);
+      27             : 
+      28        6347 :     publisher_ = nh.advertise<TopicType>(address, buffer_size, latch);
+      29             : 
+      30        6347 :     if (rate > 0.0) {
+      31             : 
+      32         659 :       throttle_ = true;
+      33             : 
+      34         659 :       throttle_min_dt_ = 1.0 / rate;
+      35             : 
+      36             :     } else {
+      37             : 
+      38        5688 :       throttle_ = false;
+      39             : 
+      40        5688 :       throttle_min_dt_ = 0;
+      41             :     }
+      42             : 
+      43        6347 :     last_time_published_ = ros::Time(0);
+      44             :   }
+      45             : 
+      46        6347 :   publisher_initialized_ = true;
+      47        6347 : }
+      48             : 
+      49             : //}
+      50             : 
+      51             : /* publish(TopicType& msg) //{ */
+      52             : 
+      53             : template <class TopicType>
+      54     5069933 : void PublisherHandler_impl<TopicType>::publish(const TopicType& msg) {
+      55             : 
+      56     5069933 :   if (!publisher_initialized_) {
+      57           0 :     return;
+      58             :   }
+      59             : 
+      60             :   {
+      61     5067225 :     std::scoped_lock lock(mutex_publisher_);
+      62             : 
+      63     5066876 :     if (throttle_) {
+      64             : 
+      65      274668 :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+      66      179561 :         return;
+      67             :       }
+      68             : 
+      69       95106 :       last_time_published_ = ros::Time::now();
+      70             :     }
+      71             : 
+      72             :     try {
+      73     4887314 :       publisher_.publish(msg);
+      74             :     }
+      75           0 :     catch (...) {
+      76           0 :       ROS_ERROR("exception caught during publishing topic '%s'", publisher_.getTopic().c_str());
+      77             :     }
+      78             :   }
+      79             : }
+      80             : 
+      81             : //}
+      82             : 
+      83             : /* publish(const boost::shared_ptr<TopicType>& msg) //{ */
+      84             : 
+      85             : template <class TopicType>
+      86             : void PublisherHandler_impl<TopicType>::publish(const boost::shared_ptr<TopicType>& msg) {
+      87             : 
+      88             :   if (!publisher_initialized_) {
+      89             :     return;
+      90             :   }
+      91             : 
+      92             :   {
+      93             :     std::scoped_lock lock(mutex_publisher_);
+      94             : 
+      95             :     if (throttle_) {
+      96             : 
+      97             :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+      98             :         return;
+      99             :       }
+     100             : 
+     101             :       last_time_published_ = ros::Time::now();
+     102             :     }
+     103             : 
+     104             :     try {
+     105             :       publisher_.publish(msg);
+     106             :     }
+     107             :     catch (...) {
+     108             :       ROS_ERROR("exception caught during publishing topic '%s'", publisher_.getTopic().c_str());
+     109             :     }
+     110             :   }
+     111             : }
+     112             : 
+     113             : //}
+     114             : 
+     115             : /* publish(const boost::shared_ptr<TopicType const>& msg) //{ */
+     116             : 
+     117             : template <class TopicType>
+     118           1 : void PublisherHandler_impl<TopicType>::publish(const boost::shared_ptr<TopicType const>& msg) {
+     119             : 
+     120           1 :   if (!publisher_initialized_) {
+     121           0 :     return;
+     122             :   }
+     123             : 
+     124             :   {
+     125           1 :     std::scoped_lock lock(mutex_publisher_);
+     126             : 
+     127           1 :     if (throttle_) {
+     128             : 
+     129           0 :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+     130           0 :         return;
+     131             :       }
+     132             : 
+     133           0 :       last_time_published_ = ros::Time::now();
+     134             :     }
+     135             : 
+     136             :     try {
+     137           1 :       publisher_.publish(msg);
+     138             :     }
+     139           0 :     catch (...) {
+     140           0 :       ROS_ERROR("exception caught during publishing topic '%s'", publisher_.getTopic().c_str());
+     141             :     }
+     142             :   }
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* getNumSubscribers(void) //{ */
+     148             : 
+     149             : template <class TopicType>
+     150             : unsigned int PublisherHandler_impl<TopicType>::getNumSubscribers(void) {
+     151             : 
+     152             :   {
+     153             :     std::scoped_lock lock(mutex_publisher_);
+     154             : 
+     155             :     return publisher_.getNumSubscribers();
+     156             :   }
+     157             : }
+     158             : 
+     159             : //}
+     160             : 
+     161             : // --------------------------------------------------------------
+     162             : // |                      PublisherHandler                      |
+     163             : // --------------------------------------------------------------
+     164             : 
+     165             : /* operator= //{ */
+     166             : 
+     167             : template <class TopicType>
+     168        7115 : PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other) {
+     169             : 
+     170        7115 :   if (this == &other) {
+     171           0 :     return *this;
+     172             :   }
+     173             : 
+     174        7115 :   if (other.impl_) {
+     175        7115 :     this->impl_ = other.impl_;
+     176             :   }
+     177             : 
+     178        7115 :   return *this;
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* copy constructor //{ */
+     184             : 
+     185             : template <class TopicType>
+     186      139491 : PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other) {
+     187             : 
+     188      139491 :   if (other.impl_) {
+     189      139491 :     this->impl_ = other.impl_;
+     190             :   }
+     191      139491 : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int &buffer_size, const bool &latch) //{ */
+     196             : 
+     197             : template <class TopicType>
+     198        6347 : PublisherHandler<TopicType>::PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+     199        6347 :                                               const double& rate) {
+     200             : 
+     201        6347 :   impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(nh, address, buffer_size, latch, rate);
+     202        6347 : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* publish(const TopicType& msg) //{ */
+     207             : 
+     208             : template <class TopicType>
+     209     5070744 : void PublisherHandler<TopicType>::publish(const TopicType& msg) {
+     210             : 
+     211     5070744 :   impl_->publish(msg);
+     212     5071644 : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* publish(const boost::shared_ptr<TopicType>& msg) //{ */
+     217             : 
+     218             : template <class TopicType>
+     219             : void PublisherHandler<TopicType>::publish(const boost::shared_ptr<TopicType>& msg) {
+     220             : 
+     221             :   impl_->publish(msg);
+     222             : }
+     223             : 
+     224             : //}
+     225             : 
+     226             : /* publish(const boost::shared_ptr<TopicType const>& msg) //{ */
+     227             : 
+     228             : template <class TopicType>
+     229           1 : void PublisherHandler<TopicType>::publish(const boost::shared_ptr<TopicType const>& msg) {
+     230             : 
+     231           1 :   impl_->publish(msg);
+     232           1 : }
+     233             : 
+     234             : //}
+     235             : 
+     236             : /* getNumSubscribers(void) //{ */
+     237             : 
+     238             : template <class TopicType>
+     239             : unsigned int PublisherHandler<TopicType>::getNumSubscribers(void) {
+     240             : 
+     241             :   return impl_->getNumSubscribers();
+     242             : }
+     243             : 
+     244             : //}
+     245             : 
+     246             : }  // namespace mrs_lib
+     247             : 
+     248             : #endif  // PUBLISHER_HANDLER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.overview.html new file mode 100644 index 0000000000..4ab1195056 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.overview.html @@ -0,0 +1,82 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..635f18b8a53237cee5f0d09d83c1fed5e41f3609 GIT binary patch literal 798 zcmV+(1L6FMP)q?p-?i-RjX(;YJ986rs_mRR+& z!Y<6?3J4QF(~^;O<*WMRe90>Mf*%@bz^-JM1R`wOATk#1l<#Kh5t#xM6ciL;M72mR zY3+(=b}l#HiYR zek5J0OjUl2b3~AfM3}NldX^B89V%|y>-BnWMVU50Nh#uTYieeBV(Jk+$~liRzI)FV z;#$bI77;o&INwVuRV0YKAW6xF26A_>#!Pm`*C-?WDSk`wD-`8fF!E4elK=F#?Q0=I z5jzWbgpbreMs4J*n#gmAcprXrs|Y_ou^R)EUnRt4e;?wADLUG08>pUbmseoD8+7$GdOSDvk4}F5 zcno{K%D|yA? zJma6R+lHy}NUmy_>Ljl>Qlt4jV;mm8`{!rI)}k-EK*F=z01|q5F~XT4GqEY86Sm~T c=jTZ84>I7)EiN$bBme*a07*qoM6N<$g7<4~+W-In literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html new file mode 100644 index 0000000000..e5ca5d1a30 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-04-25 05:42:57Functions:4747100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride> const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv> const&)2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&, int const&, double const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::asyncRun()2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)94
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)95
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)95
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)95
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)97
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)97
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)188
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)213
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)213
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)327
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)327
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)367
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)367
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)566
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)566
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)566
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)938
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)938
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1032
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html new file mode 100644 index 0000000000..ff1e5ada48 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-04-25 05:42:57Functions:4747100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv> const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride> const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)95
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)95
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)97
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)94
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)367
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)188
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)327
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)566
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)566
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)213
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)938
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)938
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)95
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)97
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)367
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)327
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)566
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)213
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&, int const&, double const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::asyncRun()2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1032
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.frameset.html new file mode 100644 index 0000000000..c6e9ac6564 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html new file mode 100644 index 0000000000..4c22c6e9a2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html @@ -0,0 +1,403 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-04-25 05:42:57Functions:4747100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef SERVICE_CLIENT_HANDLER_HPP
+       2             : #define SERVICE_CLIENT_HANDLER_HPP
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             : 
+       7             : // --------------------------------------------------------------
+       8             : // |                  ServiceClientHandler_impl                 |
+       9             : // --------------------------------------------------------------
+      10             : 
+      11             : /* ServiceClientHandler_impl(void) //{ */
+      12             : 
+      13             : template <class ServiceType>
+      14             : ServiceClientHandler_impl<ServiceType>::ServiceClientHandler_impl(void) : service_initialized_(false) {
+      15             : }
+      16             : 
+      17             : //}
+      18             : 
+      19             : /* ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address) //{ */
+      20             : 
+      21             : template <class ServiceType>
+      22        2072 : ServiceClientHandler_impl<ServiceType>::ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address) {
+      23             : 
+      24             :   {
+      25        2072 :     std::scoped_lock lock(mutex_service_client_);
+      26             : 
+      27        2072 :     service_client_ = nh.serviceClient<ServiceType>(address);
+      28             :   }
+      29             : 
+      30        2072 :   _address_       = address;
+      31        2072 :   async_attempts_ = 1;
+      32             : 
+      33             :   /* thread_oneshot_ = std::make_shared<std::thread>(std::thread(&ServiceClientHandler_impl::threadOneshot, this, true, false)); */
+      34             : 
+      35        2072 :   service_initialized_ = true;
+      36        2072 : }
+      37             : 
+      38             : //}
+      39             : 
+      40             : /* call(ServiceType& srv) //{ */
+      41             : 
+      42             : template <class ServiceType>
+      43        1132 : bool ServiceClientHandler_impl<ServiceType>::call(ServiceType& srv) {
+      44             : 
+      45        1132 :   if (!service_initialized_) {
+      46           0 :     return false;
+      47             :   }
+      48             : 
+      49        1132 :   return service_client_.call(srv);
+      50             : }
+      51             : 
+      52             : //}
+      53             : 
+      54             : /* call(ServiceType& srv, const int& attempts) //{ */
+      55             : 
+      56             : template <class ServiceType>
+      57             : bool ServiceClientHandler_impl<ServiceType>::call(ServiceType& srv, const int& attempts) {
+      58             : 
+      59             :   if (!service_initialized_) {
+      60             :     return false;
+      61             :   }
+      62             : 
+      63             :   std::scoped_lock lock(mutex_service_client_);
+      64             : 
+      65             :   bool success = false;
+      66             :   int  counter = 0;
+      67             : 
+      68             :   while (!success && ros::ok()) {
+      69             : 
+      70             :     success = service_client_.call(srv);
+      71             : 
+      72             :     if (!success) {
+      73             :       ROS_ERROR("[%s]: failed to call service to '%s'", ros::this_node::getName().c_str(), _address_.c_str());
+      74             :     }
+      75             : 
+      76             :     if (++counter >= attempts) {
+      77             :       break;
+      78             :     }
+      79             :   }
+      80             : 
+      81             :   return success;
+      82             : }
+      83             : 
+      84             : //}
+      85             : 
+      86             : /* call(ServiceType& srv, const int& attempts, const double& repeat_delay) //{ */
+      87             : 
+      88             : template <class ServiceType>
+      89           2 : bool ServiceClientHandler_impl<ServiceType>::call(ServiceType& srv, const int& attempts, const double& repeat_delay) {
+      90             : 
+      91           2 :   if (!service_initialized_) {
+      92           0 :     return false;
+      93             :   }
+      94             : 
+      95           2 :   std::scoped_lock lock(mutex_service_client_);
+      96             : 
+      97           2 :   bool success = false;
+      98           2 :   int  counter = 0;
+      99             : 
+     100           8 :   while (!success && ros::ok()) {
+     101             : 
+     102           6 :     success = service_client_.call(srv);
+     103             : 
+     104           6 :     if (!success) {
+     105           4 :       ROS_ERROR("[%s]: failed to call service to '%s'", ros::this_node::getName().c_str(), _address_.c_str());
+     106             :     }
+     107             : 
+     108           6 :     if (++counter >= attempts) {
+     109           0 :       break;
+     110             :     }
+     111             : 
+     112           6 :     ros::Duration(repeat_delay).sleep();
+     113             :   }
+     114             : 
+     115           2 :   return success;
+     116             : }
+     117             : 
+     118             : //}
+     119             : 
+     120             : /* callAsync(ServiceType& srv) //{ */
+     121             : 
+     122             : template <class ServiceType>
+     123             : std::future<ServiceType> ServiceClientHandler_impl<ServiceType>::callAsync(ServiceType& srv) {
+     124             : 
+     125             :   {
+     126             :     std::scoped_lock lock(mutex_async_);
+     127             : 
+     128             :     async_data_         = srv;
+     129             :     async_attempts_     = 1;
+     130             :     async_repeat_delay_ = 0;
+     131             :   }
+     132             : 
+     133             :   return std::async(std::launch::async, &ServiceClientHandler_impl::asyncRun, this);
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* callAsync(ServiceType& srv, const int& attempts) //{ */
+     139             : 
+     140             : template <class ServiceType>
+     141           2 : std::future<ServiceType> ServiceClientHandler_impl<ServiceType>::callAsync(ServiceType& srv, const int& attempts) {
+     142             : 
+     143             :   {
+     144           2 :     std::scoped_lock lock(mutex_async_);
+     145             : 
+     146           2 :     async_data_         = srv;
+     147           2 :     async_attempts_     = attempts;
+     148           2 :     async_repeat_delay_ = 0;
+     149             :   }
+     150             : 
+     151           2 :   return std::async(std::launch::async, &ServiceClientHandler_impl::asyncRun, this);
+     152             : }
+     153             : 
+     154             : //}
+     155             : 
+     156             : /* callAsync(ServiceType& srv, const int& attempts, const double &repeat_delay) //{ */
+     157             : 
+     158             : template <class ServiceType>
+     159             : std::future<ServiceType> ServiceClientHandler_impl<ServiceType>::callAsync(ServiceType& srv, const int& attempts, const double& repeat_delay) {
+     160             : 
+     161             :   {
+     162             :     std::scoped_lock lock(mutex_async_);
+     163             : 
+     164             :     async_data_         = srv;
+     165             :     async_attempts_     = attempts;
+     166             :     async_repeat_delay_ = repeat_delay;
+     167             :   }
+     168             : 
+     169             :   return std::async(std::launch::async, &ServiceClientHandler_impl::asyncRun, this);
+     170             : }
+     171             : 
+     172             : //}
+     173             : 
+     174             : /* asyncRun(void) //{ */
+     175             : 
+     176             : template <class ServiceType>
+     177           2 : ServiceType ServiceClientHandler_impl<ServiceType>::asyncRun(void) {
+     178             : 
+     179           2 :   ServiceType async_data;
+     180             :   int         async_attempts;
+     181             : 
+     182             :   {
+     183           2 :     std::scoped_lock lock(mutex_async_);
+     184             : 
+     185           2 :     async_data          = async_data_;
+     186           2 :     async_attempts      = async_attempts_;
+     187           2 :     async_repeat_delay_ = async_repeat_delay_;
+     188             :   }
+     189             : 
+     190           2 :   call(async_data, async_attempts, async_repeat_delay_);
+     191             : 
+     192           4 :   return async_data;
+     193             : }
+     194             : 
+     195             : //}
+     196             : 
+     197             : // --------------------------------------------------------------
+     198             : // |                    ServiceClientHandler                    |
+     199             : // --------------------------------------------------------------
+     200             : 
+     201             : /* operator= //{ */
+     202             : 
+     203             : template <class ServiceType>
+     204        1978 : ServiceClientHandler<ServiceType>& ServiceClientHandler<ServiceType>::operator=(const ServiceClientHandler<ServiceType>& other) {
+     205             : 
+     206        1978 :   if (this == &other) {
+     207           0 :     return *this;
+     208             :   }
+     209             : 
+     210        1978 :   if (other.impl_) {
+     211        1978 :     this->impl_ = other.impl_;
+     212             :   }
+     213             : 
+     214        1978 :   return *this;
+     215             : }
+     216             : 
+     217             : //}
+     218             : 
+     219             : /* copy constructor //{ */
+     220             : 
+     221             : template <class ServiceType>
+     222             : ServiceClientHandler<ServiceType>::ServiceClientHandler(const ServiceClientHandler<ServiceType>& other) {
+     223             : 
+     224             :   if (other.impl_) {
+     225             :     this->impl_ = other.impl_;
+     226             :   }
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* ServiceClientHandler(ros::NodeHandle& nh, const std::string& address) //{ */
+     232             : 
+     233             : template <class ServiceType>
+     234        1978 : ServiceClientHandler<ServiceType>::ServiceClientHandler(ros::NodeHandle& nh, const std::string& address) {
+     235             : 
+     236        1978 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     237        1978 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : /* initialize(ros::NodeHandle& nh, const std::string& address) //{ */
+     242             : 
+     243             : template <class ServiceType>
+     244          94 : void ServiceClientHandler<ServiceType>::initialize(ros::NodeHandle& nh, const std::string& address) {
+     245             : 
+     246          94 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     247          94 : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* call(ServiceType& srv) //{ */
+     252             : 
+     253             : template <class ServiceType>
+     254        1132 : bool ServiceClientHandler<ServiceType>::call(ServiceType& srv) {
+     255             : 
+     256        1132 :   return impl_->call(srv);
+     257             : }
+     258             : 
+     259             : //}
+     260             : 
+     261             : /* call(ServiceType& srv, const int& attempts) //{ */
+     262             : 
+     263             : template <class ServiceType>
+     264             : bool ServiceClientHandler<ServiceType>::call(ServiceType& srv, const int& attempts) {
+     265             : 
+     266             :   return impl_->call(srv, attempts);
+     267             : }
+     268             : 
+     269             : //}
+     270             : 
+     271             : /* call(ServiceType& srv, const int& attempts, const double& repeat_delay) //{ */
+     272             : 
+     273             : template <class ServiceType>
+     274             : bool ServiceClientHandler<ServiceType>::call(ServiceType& srv, const int& attempts, const double& repeat_delay) {
+     275             : 
+     276             :   return impl_->call(srv, attempts, repeat_delay);
+     277             : }
+     278             : 
+     279             : //}
+     280             : 
+     281             : /* callAsync(ServiceType& srv) //{ */
+     282             : 
+     283             : template <class ServiceType>
+     284             : std::future<ServiceType> ServiceClientHandler<ServiceType>::callAsync(ServiceType& srv) {
+     285             : 
+     286             :   std::future<ServiceType> res = impl_->callAsync(srv);
+     287             : 
+     288             :   return res;
+     289             : }
+     290             : 
+     291             : //}
+     292             : 
+     293             : /* callAsync(ServiceType& srv, const int& attempts) //{ */
+     294             : 
+     295             : template <class ServiceType>
+     296           2 : std::future<ServiceType> ServiceClientHandler<ServiceType>::callAsync(ServiceType& srv, const int& attempts) {
+     297             : 
+     298           2 :   std::future<ServiceType> res = impl_->callAsync(srv, attempts);
+     299             : 
+     300           2 :   return res;
+     301             : }
+     302             : 
+     303             : //}
+     304             : 
+     305             : /* callAsync(ServiceType& srv, const int& attempts, const double& repeat_delay) //{ */
+     306             : 
+     307             : template <class ServiceType>
+     308             : std::future<ServiceType> ServiceClientHandler<ServiceType>::callAsync(ServiceType& srv, const int& attempts, const double& repeat_delay) {
+     309             : 
+     310             :   std::future<ServiceType> res = impl_->callAsync(srv, attempts, repeat_delay);
+     311             : 
+     312             :   return res;
+     313             : }
+     314             : 
+     315             : //}
+     316             : 
+     317             : }  // namespace mrs_lib
+     318             : 
+     319             : #endif  // SERVICE_CLIENT_HANDLER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.overview.html new file mode 100644 index 0000000000..ab6ff399da --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.overview.html @@ -0,0 +1,100 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..263b73ad1380cf9f7a8ef86c51aca552078dfaf1 GIT binary patch literal 969 zcmV;)12+7LP)Wqk9Iv^@lP(dY(=QXOzU}^O$n3W+JqlhhKurdbsVPJ$4 zm2sw#fHn(q8wtvc3@&CusskcExa5-;CL|9hAa2i^A<8=Aw(a}=Z{L45n3k=@;Th4i zo+G;AC3x?_TV}BL=oh6ew&M%93cw*aU1|285l zT!NU`Mp*~6s%7#VjJVE+;1pg)`&RYb!*R9eBK&bfdM-)ECo1pbkef4EZ$u;FTpg)P zx3}9BbjHow&pHv-#>@V^FkzWGq7i_@V1BnXBT7cyp+S7ZdWwYd9?^X?Sx?~`3g+AB zi?{EDo+51^>hz*Duv_mvWO+jOvI5%_itJ)oVlEcCI)xaE{P+`cY9&_gK=;7v_ zg@l7f#Cmv|5B_Qn2e&pFhJ#l4pTj{!@4{Ust_Bh7;kDcl6MM*J$V{;&kWhNjcS!m@ zX4Bib=l$iw{G;lCNmYo9Mb=aKsEax3d?wu;Sze#!6tcc6k#V+yRHD*pb5cs;XTwjM zJKXO?L{du0Qb;+o5JyeY3<%(%%UefHGE10sW!5aF0me~nVH0i;^<5TL9l1NA@qtxXjPZ-Hy|8-Suo%AbI@I=D1MDetRyXI_3l`vS}O#^lU5 zmu0@;ASZ`RpG3To0T|}uDQ!LTkw$iqE6sxg*#assQV?bPCT_M`SRU@fW91^^N5(Y^ rzi2W3O>xaxC_|mFwRLjam4SZ&Pzvjg528x=00000NkvXXu0mjf<=M*E literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html new file mode 100644 index 0000000000..81c507be2a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html @@ -0,0 +1,4256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-04-25 05:42:57Functions:493104447.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::stop()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::lastMsgTime() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const5
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const9
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::peekMsg() const22
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)89
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)89
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const95
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)> const&)96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().296
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)> const&)96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().296
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const97
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const>)> const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().299
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::start()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const>)> const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().299
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const99
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const99
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()100
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()100
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const108
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const115
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)> const&)181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::start()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)> const&)181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const181
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const185
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::lastMsgTime() const185
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::start()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::start()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const188
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)> const&)192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::start()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)> const&)192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)> const&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::start()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)> const&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const219
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const260
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::lastMsgTime() const260
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()264
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()264
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const264
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const264
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::start()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const282
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)> const&)290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::start()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)> const&)290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const291
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)> const&)301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::start()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)> const&)301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const301
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)> const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::start()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)> const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2313
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)331
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)331
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const364
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const>)> const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::start()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const>)> const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const365
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)> const&)374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::start()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)> const&)374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const374
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)> const&)376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::start()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)> const&)376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const376
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)392
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)392
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::start()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::start()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const407
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const412
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const511
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)> const&)562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)> const&)562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2562
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)574
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)574
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const695
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const755
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const792
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1683
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1683
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1684
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1684
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2138
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2138
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2492
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2492
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3113
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const3753
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5145
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5156
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8172
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8173
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const13935
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()13938
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()13940
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const13942
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)14532
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)14532
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16455
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const16455
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)17353
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)17353
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const17560
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17561
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18482
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const18482
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18685
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const18685
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18711
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const18711
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)22551
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)22551
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::lastMsgTime() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const36861
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()44774
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const44780
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const44781
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()44783
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const52850
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const52850
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)57319
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)57319
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const66594
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const66618
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()68516
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const68537
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const68537
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()68540
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const70905
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const70922
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)72724
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)72884
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)74058
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)74274
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const107071
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const107071
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)136792
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)136792
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const143351
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const143362
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()143957
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const143961
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()143965
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const143966
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const166115
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const166115
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const183682
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()183683
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()183687
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const183687
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)187729
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)187730
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)188206
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)188209
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)188713
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)188728
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)200254
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)200281
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const207966
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const207966
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()224350
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()224351
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const224353
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const224354
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const242294
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const242296
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)282869
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)283305
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)314889
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)314961
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)317148
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)317294
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)346947
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)348731
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const379086
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const379088
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const388604
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()388779
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()388792
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const388843
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::lastMsgTime() const410979
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const411087
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)475394
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)475520
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)503629
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)503812
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const517151
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const517410
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const529116
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const529123
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)594392
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)594659
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()708487
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()708494
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const708512
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const708534
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const907214
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const907234
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)936940
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)937296
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1139073
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1139160
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html new file mode 100644 index 0000000000..055274403d --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html @@ -0,0 +1,4256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-04-25 05:42:57Functions:493104447.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)283305
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)> const&)374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)282869
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::start()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)> const&)374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2374
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)57319
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)57319
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2188
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)314961
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()13940
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)> const&)181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)314889
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::start()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()13938
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)> const&)181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2181
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)187730
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()100
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)> const&)96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().296
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)187729
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()100
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)> const&)96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().296
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)594659
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()183687
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)> const&)301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)594392
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::start()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()183683
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)> const&)301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2301
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1139073
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()708487
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)> const&)562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1139160
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)6
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()708494
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)> const&)562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2562
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)937296
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()264
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)> const&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)936940
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::start()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()264
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)> const&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)188206
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)> const&)192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)188209
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::start()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)> const&)192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2192
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)348731
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()388779
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const>)> const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)346947
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::start()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()388792
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const>)> const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2365
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)317294
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()44774
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)> const&)376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)317148
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::start()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()44783
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)> const&)376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2376
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)200254
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)200281
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::start()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2188
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)188728
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const>)> const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().299
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)188713
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::start()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const>)> const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().299
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)22551
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)22551
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)392
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)392
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)331
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)331
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5145
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)> const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5156
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::start()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)> const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2313
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)14532
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)14532
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)136792
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)136792
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)72884
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()68516
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)72724
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::start()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()68540
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2407
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2138
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2138
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)574
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)574
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1683
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1683
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3113
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3113
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)89
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)89
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)74274
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()143957
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)74058
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::start()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()143965
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2407
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1684
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1684
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::start()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().294
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::start()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().294
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8173
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8172
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl().29
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)475520
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)475394
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::start()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2282
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)503812
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()224350
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)> const&)290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)503629
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::start()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()224351
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)> const&)290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2290
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)17353
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)17353
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::start()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2188
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const374
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const188
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17561
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const13935
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const17560
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const13942
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const181
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16455
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const16455
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const97
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const379088
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const183682
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const379086
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const183687
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const301
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const907234
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const708512
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const115
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const907214
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const708534
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const755
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const185
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18685
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const264
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::lastMsgTime() const185
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const18685
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const264
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const166115
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const166115
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const192
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const411087
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const107071
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const517410
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const388604
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::lastMsgTime() const410979
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const107071
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const517151
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const388843
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const365
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const66618
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const44781
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const66594
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const44780
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const376
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const188
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const99
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const260
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2492
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::lastMsgTime() const260
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2492
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18711
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const99
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const18711
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const412
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const95
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const95
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const70905
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const68537
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const70922
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const68537
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const407
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18482
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const18482
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const143351
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const143961
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const104
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const143362
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const143966
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const511
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const94
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const207966
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const242294
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const207966
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const242296
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const282
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const529116
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const224353
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const529123
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const224354
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const291
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const52850
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::lastMsgTime() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const52850
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const188
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.frameset.html new file mode 100644 index 0000000000..19a3af8db0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html new file mode 100644 index 0000000000..4219926b22 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html @@ -0,0 +1,413 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-04-25 05:42:57Functions:493104447.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : 
+       3             : #ifndef SUBSCRIBE_HANDLER_HPP
+       4             : #define SUBSCRIBE_HANDLER_HPP
+       5             : 
+       6             : #include <mrs_lib/subscribe_handler.h>
+       7             : #include <mrs_lib/timer.h>
+       8             : #include <mutex>
+       9             : #include <condition_variable>
+      10             : 
+      11             : namespace mrs_lib
+      12             : {
+      13             :   /* SubscribeHandler::Impl class //{ */
+      14             :   // implements the constructor, getMsg() method and data_callback method (non-thread-safe)
+      15             :   template <typename MessageType>
+      16             :   class SubscribeHandler<MessageType>::Impl
+      17             :   {
+      18             :   public:
+      19             :     using timeout_callback_t = typename SubscribeHandler<MessageType>::timeout_callback_t;
+      20             :     using message_callback_t = typename SubscribeHandler<MessageType>::message_callback_t;
+      21             :     using data_callback_t = std::function<void(const typename MessageType::ConstPtr&)>;
+      22             : 
+      23             :   private:
+      24             :     friend class SubscribeHandler<MessageType>;
+      25             : 
+      26             :   public:
+      27             :     /* constructor //{ */
+      28        6184 :     Impl(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+      29        6184 :         : m_nh(options.nh),
+      30        6184 :           m_topic_name(options.topic_name),
+      31        6184 :           m_node_name(options.node_name),
+      32             :           m_got_data(false),
+      33             :           m_new_data(false),
+      34             :           m_used_data(false),
+      35        6184 :           m_timeout_manager(options.timeout_manager),
+      36             :           m_latest_message_time(0),
+      37             :           m_latest_message(nullptr),
+      38             :           m_message_callback(message_callback),
+      39        6184 :           m_queue_size(options.queue_size),
+      40        6184 :           m_transport_hints(options.transport_hints)
+      41             :     {
+      42        6184 :       if (options.no_message_timeout != mrs_lib::no_timeout)
+      43             :       {
+      44             :         // initialize a new TimeoutManager if not provided by the user
+      45          78 :         if (!m_timeout_manager)
+      46          78 :           m_timeout_manager = std::make_shared<mrs_lib::TimeoutManager>(m_nh, ros::Rate(2.0/options.no_message_timeout.toSec()));
+      47             : 
+      48             :         // initialize the callback for the TimeoutManager
+      49         156 :         std::function<void(const ros::Time&)> timeout_mgr_callback;
+      50          78 :         if (options.timeout_callback)
+      51           1 :           timeout_mgr_callback = std::bind(options.timeout_callback, topicName(), std::placeholders::_1);
+      52             :         else
+      53          77 :           timeout_mgr_callback = std::bind(&Impl::default_timeout_callback, this, topicName(), std::placeholders::_1);
+      54             : 
+      55             :         // register the timeout callback with the TimeoutManager
+      56          78 :         m_timeout_id = m_timeout_manager->registerNew(options.no_message_timeout, timeout_mgr_callback);
+      57             :       }
+      58             : 
+      59       18552 :       const std::string msg = "Subscribed to topic '" + m_topic_name + "' -> '" + topicName() + "'";
+      60        6184 :       if (m_node_name.empty())
+      61           1 :         ROS_INFO_STREAM(msg);
+      62             :       else
+      63        6183 :         ROS_INFO_STREAM("[" << m_node_name << "]: " << msg);
+      64        6184 :     }
+      65             :     //}
+      66             : 
+      67        6184 :     virtual ~Impl() = default;
+      68             : 
+      69             :   public:
+      70             :     /* getMsg() method //{ */
+      71     2015384 :     virtual typename MessageType::ConstPtr getMsg()
+      72             :     {
+      73     2015384 :       m_new_data = false;
+      74     2015384 :       m_used_data = true;
+      75     2015384 :       return peekMsg();
+      76             :     }
+      77             :     //}
+      78             : 
+      79             :     /* peekMsg() method //{ */
+      80     2015481 :     virtual typename MessageType::ConstPtr peekMsg() const
+      81             :     {
+      82             :       /* assert(m_got_data); */
+      83             :       /* if (!m_got_data) */
+      84             :       /*   ROS_ERROR("[%s]: No data received yet from topic '%s' (forgot to check hasMsg()?)! Returning empty message.", m_node_name.c_str(), */
+      85             :       /*             topicName().c_str()); */
+      86     2015481 :       return m_latest_message;
+      87             :     }
+      88             :     //}
+      89             : 
+      90             :     /* hasMsg() method //{ */
+      91     2767879 :     virtual bool hasMsg() const
+      92             :     {
+      93     2767879 :       return m_got_data;
+      94             :     }
+      95             :     //}
+      96             : 
+      97             :     /* newMsg() method //{ */
+      98      517151 :     virtual bool newMsg() const
+      99             :     {
+     100      517151 :       return m_new_data;
+     101             :     }
+     102             :     //}
+     103             : 
+     104             :     /* usedMsg() method //{ */
+     105           0 :     virtual bool usedMsg() const
+     106             :     {
+     107           0 :       return m_used_data;
+     108             :     }
+     109             :     //}
+     110             : 
+     111             :     /* waitForNew() method //{ */
+     112           0 :     virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout)
+     113             :     {
+     114             :       // convert the ros type to chrono type
+     115           0 :       const std::chrono::duration<float> chrono_timeout(timeout.toSec());
+     116             :       // lock the mutex guarding the m_new_data flag
+     117           0 :       std::unique_lock lock(m_new_data_mtx);
+     118             :       // if new data is available, return immediately, otherwise attempt to wait for new data using the respective condition variable
+     119           0 :       if (m_new_data)
+     120           0 :         return getMsg();
+     121           0 :       else if (m_new_data_cv.wait_for(lock, chrono_timeout) == std::cv_status::no_timeout && m_new_data)
+     122           0 :         return getMsg();
+     123             :       else
+     124           0 :         return nullptr;
+     125             :     };
+     126             :     //}
+     127             : 
+     128             :     /* lastMsgTime() method //{ */
+     129      656436 :     virtual ros::Time lastMsgTime() const
+     130             :     {
+     131      656436 :       return m_latest_message_time;
+     132             :     };
+     133             :     //}
+     134             : 
+     135             :     /* topicName() method //{ */
+     136        6582 :     virtual std::string topicName() const
+     137             :     {
+     138        6582 :       std::string ret = m_sub.getTopic();
+     139        6582 :       if (ret.empty())
+     140        6262 :         ret = m_nh.resolveName(m_topic_name);
+     141        6582 :       return ret;
+     142             :     }
+     143             :     //}
+     144             : 
+     145             :     /* start() method //{ */
+     146        6188 :     virtual void start()
+     147             :     {
+     148        6188 :       if (m_timeout_manager)
+     149          82 :         m_timeout_manager->start(m_timeout_id);
+     150        6188 :       m_sub = m_nh.subscribe(m_topic_name, m_queue_size, &Impl::data_callback, this, m_transport_hints);
+     151        6188 :     }
+     152             :     //}
+     153             : 
+     154             :     /* stop() method //{ */
+     155           5 :     virtual void stop()
+     156             :     {
+     157           5 :       if (m_timeout_manager)
+     158           5 :         m_timeout_manager->pause(m_timeout_id);
+     159           5 :       m_sub.shutdown();
+     160           5 :     }
+     161             :     //}
+     162             : 
+     163             :   protected:
+     164             :     ros::NodeHandle m_nh;
+     165             :     ros::Subscriber m_sub;
+     166             : 
+     167             :   protected:
+     168             :     std::string m_topic_name;
+     169             :     std::string m_node_name;
+     170             : 
+     171             :   protected:
+     172             :     bool m_got_data;   // whether any data was received
+     173             : 
+     174             :     mutable std::mutex m_new_data_mtx;
+     175             :     mutable std::condition_variable m_new_data_cv;
+     176             :     bool m_new_data;   // whether new data was received since last call to get_data
+     177             : 
+     178             :     bool m_used_data;  // whether get_data was successfully called at least once
+     179             : 
+     180             :   protected:
+     181             :     std::shared_ptr<mrs_lib::TimeoutManager> m_timeout_manager;
+     182             :     mrs_lib::TimeoutManager::timeout_id_t m_timeout_id;
+     183             : 
+     184             :   protected:
+     185             :     ros::Time m_latest_message_time;
+     186             :     typename MessageType::ConstPtr m_latest_message;
+     187             :     message_callback_t m_message_callback;
+     188             : 
+     189             :   private:
+     190             :     uint32_t m_queue_size;
+     191             :     ros::TransportHints m_transport_hints;
+     192             : 
+     193             :   protected:
+     194             :     /* default_timeout_callback() method //{ */
+     195           6 :     void default_timeout_callback(const std::string& topic_name, const ros::Time& last_msg)
+     196             :     {
+     197           6 :       const ros::Duration since_msg = (ros::Time::now() - last_msg);
+     198           6 :       const auto n_pubs = m_sub.getNumPublishers();
+     199          18 :       const std::string txt = "Did not receive any message from topic '" + topic_name + "' for " + std::to_string(since_msg.toSec()) + "s ("
+     200             :                               + std::to_string(n_pubs) + " publishers on this topic)";
+     201           6 :       if (m_node_name.empty())
+     202           0 :         ROS_WARN_STREAM(txt);
+     203             :       else
+     204           6 :         ROS_WARN_STREAM("[" << m_node_name << "]: " << txt);
+     205           6 :     }
+     206             :     //}
+     207             : 
+     208             :     /* process_new_message() method //{ */
+     209     6094964 :     void process_new_message(const typename MessageType::ConstPtr& msg)
+     210             :     {
+     211     6094964 :       m_latest_message_time = ros::Time::now();
+     212     6100750 :       m_latest_message = msg;
+     213             :       // If the message callback is registered, the new data will immediately be processed,
+     214             :       // so reset the flag. Otherwise, set the flag.
+     215     6097974 :       m_new_data = !m_message_callback;
+     216     6092468 :       m_got_data = true;
+     217     6092468 :       m_new_data_cv.notify_one();
+     218     6100276 :     }
+     219             :     //}
+     220             : 
+     221             :     /* data_callback() method //{ */
+     222           0 :     virtual void data_callback(const typename MessageType::ConstPtr& msg)
+     223             :     {
+     224             :       {
+     225           0 :         std::lock_guard lck(m_new_data_mtx);
+     226           0 :         if (m_timeout_manager)
+     227           0 :           m_timeout_manager->reset(m_timeout_id);
+     228           0 :         process_new_message(msg);
+     229             :       }
+     230             : 
+     231             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     232           0 :       if (m_message_callback)
+     233           0 :         m_message_callback(msg);
+     234           0 :     }
+     235             :     //}
+     236             :   };
+     237             :   //}
+     238             : 
+     239             :   /* SubscribeHandler_threadsafe class //{ */
+     240             :   template <typename MessageType>
+     241             :   class SubscribeHandler<MessageType>::ImplThreadsafe : public SubscribeHandler<MessageType>::Impl
+     242             :   {
+     243             :   private:
+     244             :     using impl_class_t = SubscribeHandler<MessageType>::Impl;
+     245             : 
+     246             :   public:
+     247             :     using timeout_callback_t = typename impl_class_t::timeout_callback_t;
+     248             :     using message_callback_t = typename impl_class_t::message_callback_t;
+     249             : 
+     250             :     friend class SubscribeHandler<MessageType>;
+     251             : 
+     252             :   public:
+     253        6184 :     ImplThreadsafe(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+     254        6184 :         : impl_class_t::Impl(options, message_callback)
+     255             :     {
+     256        6184 :     }
+     257             : 
+     258             :   public:
+     259     2767890 :     virtual bool hasMsg() const override
+     260             :     {
+     261     5535674 :       std::lock_guard lck(m_mtx);
+     262     5535839 :       return impl_class_t::hasMsg();
+     263             :     }
+     264      517410 :     virtual bool newMsg() const override
+     265             :     {
+     266     1033830 :       std::lock_guard lck(m_mtx);
+     267     1033322 :       return impl_class_t::newMsg();
+     268             :     }
+     269           0 :     virtual bool usedMsg() const override
+     270             :     {
+     271           0 :       std::lock_guard lck(m_mtx);
+     272           0 :       return impl_class_t::usedMsg();
+     273             :     }
+     274     2015328 :     virtual typename MessageType::ConstPtr getMsg() override
+     275             :     {
+     276     4030579 :       std::lock_guard lck(m_mtx);
+     277     4030143 :       return impl_class_t::getMsg();
+     278             :     }
+     279     2015202 :     virtual typename MessageType::ConstPtr peekMsg() const override
+     280             :     {
+     281     4030072 :       std::lock_guard lck(m_mtx);
+     282     4029847 :       return impl_class_t::peekMsg();
+     283             :     }
+     284      656544 :     virtual ros::Time lastMsgTime() const override
+     285             :     {
+     286     1312930 :       std::lock_guard lck(m_mtx);
+     287     1312822 :       return impl_class_t::lastMsgTime();
+     288             :     };
+     289         319 :     virtual std::string topicName() const override
+     290             :     {
+     291         639 :       std::lock_guard lck(m_mtx);
+     292         640 :       return impl_class_t::topicName();
+     293             :     };
+     294        6188 :     virtual void start() override
+     295             :     {
+     296       12376 :       std::lock_guard lck(m_mtx);
+     297       12376 :       return impl_class_t::start();
+     298             :     }
+     299           5 :     virtual void stop() override
+     300             :     {
+     301          10 :       std::lock_guard lck(m_mtx);
+     302          10 :       return impl_class_t::stop();
+     303             :     }
+     304             : 
+     305       12368 :     virtual ~ImplThreadsafe() override = default;
+     306             : 
+     307             :   protected:
+     308     6098599 :     virtual void data_callback(const typename MessageType::ConstPtr& msg) override
+     309             :     {
+     310             :       {
+     311    12195578 :         std::scoped_lock lck(m_mtx, this->m_new_data_mtx);
+     312     6079522 :         if (this->m_timeout_manager)
+     313      149668 :           this->m_timeout_manager->reset(this->m_timeout_id);
+     314     6051077 :         impl_class_t::process_new_message(msg);
+     315             :       }
+     316             : 
+     317             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     318     6080381 :       if (this->m_message_callback)
+     319     3343028 :         impl_class_t::m_message_callback(msg);
+     320     6072982 :     }
+     321             : 
+     322             :   private:
+     323             :     mutable std::recursive_mutex m_mtx;
+     324             :   };
+     325             :   //}
+     326             : 
+     327             : }  // namespace mrs_lib
+     328             : 
+     329             : #endif  // SUBSCRIBE_HANDLER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html new file mode 100644 index 0000000000..02a4b3f18a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html @@ -0,0 +1,103 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..039a0b123a5be30b6b86ab27f511bc7c3edec5e5 GIT binary patch literal 1372 zcmV-i1*7_jP)PbXF zRA_$hj4@iQ$tY%%8HH^zS zW6cEpSWW-;D~SoWLPH?$vi4?$WWPqq&<)@QNwyBXCKhLurC>WWYYu4IJlu$ zh0YNwZba1sMI@ur1}1BoodzD`chMU0CaKq2J9ixq7b9FNBgj=2+oknPWgKADO5G|$ z>eIT=*2XV9fVDuGImIaQ6XuE50Sgsx4$L#=nuacVL?du^pqon%e$h)2Vvk6+u8nZs zjx+Zqpu{l^MF&#YAg*mDmw38zD>niv>=>g1Y{)AkW3Alcmgy#!WyFrOy_0myUJ6_8 zl??RO5dbY8Iw~0t^PG{`_6CpnB$bS^2Cw5*r3_Ao`HaTz5^*V>WB}_WkzB(11Pgh{ zwF5T;n+?!GY1~KxbH>ImZg*A98Oz(|zj=OvdFBhi?Y8gxW8d$`m;bmC z;{ZOWe`|ra9q8@cD{_kOGtV@Bl6jIsJAQnWUo^@@-Ue2%Sn>~9-!O~45hB|+0PhF# zktMx@bVJXyga{Yj1HtF|Ka}ySkF8iDdwn{0W;U4X8z7{D=D(l^)j;B_@EHb zdvD;*W_H=wUCK(Siv5XYk4v0A>u)c6({-u&<7$5~OX^|8?%vajS5_rekW=7_^&h}#8KHO$DWFb9 z$XyNU*`aiHp)yKF7hT|1BiGss3zUS?X)AYbSqF9y1t0^&LcR*`syj6#D~ zhi2rCwpMspEJoTa1-O{QlrW!JDi?GDnS1Zsr*&W)k!5&&My;fM5F zeG#ZYx^-W(*vofU8BnJ+I>@2Ysb?|(H`@!F)+e#Ysy5BPqfC2-M0L=$XGmpp<+ixJ zJ{B26<*ex|2(_txg=g5~eRQwlBg&BXq}0lwBaMNTKB5fS+!vmG3=bf_rVLe7D3!6- zDyl}nw<|-RO6iwo_M+4$lzEx8c%XPp4|e2A-Nr6TAaV4>F{tgYn#RyYn&o;lW?FG= zwXUZO!Ft0ayWtv&90CDPudjtwT&FZWxrWM&fbYm3<;_%-^cyno{9aucTl%v9o+2CG eIJickaQ^@zmyh{xOPAyT0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::ThreadTimer<obj_t>(ros::NodeHandle const&, ros::Rate const&, void (obj_t::*)(ros::TimerEvent const&), obj_t*, bool, bool)8
mrs_lib::ThreadTimer::ThreadTimer<obj_t>(ros::NodeHandle const&, ros::Duration const&, void (obj_t::*)(ros::TimerEvent const&), obj_t*, bool, bool)8
mrs_lib::ROSTimer::ROSTimer<obj_t>(ros::NodeHandle const&, ros::Rate const&, void (obj_t::*)(ros::TimerEvent const&), obj_t*, bool, bool)8
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.func.html b/mrs_lib/include/mrs_lib/impl/timer.hpp.func.html new file mode 100644 index 0000000000..e9a9621b0c --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::ThreadTimer<obj_t>(ros::NodeHandle const&, ros::Rate const&, void (obj_t::*)(ros::TimerEvent const&), obj_t*, bool, bool)8
mrs_lib::ThreadTimer::ThreadTimer<obj_t>(ros::NodeHandle const&, ros::Duration const&, void (obj_t::*)(ros::TimerEvent const&), obj_t*, bool, bool)8
mrs_lib::ROSTimer::ROSTimer<obj_t>(ros::NodeHandle const&, ros::Rate const&, void (obj_t::*)(ros::TimerEvent const&), obj_t*, bool, bool)8
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.frameset.html new file mode 100644 index 0000000000..25b7f54de2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html new file mode 100644 index 0000000000..efd93e764f --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html @@ -0,0 +1,180 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_TIMER_HPP
+       2             : #define MRS_TIMER_HPP
+       3             : 
+       4             : // | ------------------------ ROSTimer ------------------------ |
+       5             : 
+       6             : /* ROSTimer constructors //{ */
+       7             : 
+       8             : // duration + oneshot + autostart
+       9             : #include <chrono>
+      10             : #include <mutex>
+      11             : template <class ObjectType>
+      12             : ROSTimer::ROSTimer(const ros::NodeHandle& nh, const ros::Duration& duration, void (ObjectType::*const callback)(const ros::TimerEvent&),
+      13             :                    ObjectType* const obj, const bool oneshot, const bool autostart) {
+      14             : 
+      15             :   this->timer_ = std::make_unique<ros::Timer>(nh.createTimer(duration, callback, obj, oneshot, autostart));
+      16             : }
+      17             : 
+      18             : // rate + oneshot + autostart
+      19             : template <class ObjectType>
+      20           8 : ROSTimer::ROSTimer(const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+      21           8 :                    const bool oneshot, const bool autostart) {
+      22             : 
+      23           8 :   this->timer_ = std::make_unique<ros::Timer>(nh.createTimer(rate, callback, obj, oneshot, autostart));
+      24           8 : }
+      25             : 
+      26             : //}
+      27             : 
+      28             : // | ----------------------- ThreadTimer ---------------------- |
+      29             : 
+      30             : /* class ThreadTimer::Impl //{ */
+      31             : 
+      32             : class ThreadTimer::Impl {
+      33             : public:
+      34             :   Impl(const std::function<void(const ros::TimerEvent&)>& callback, const ros::Duration& delay_dur, const bool oneshot);
+      35             :   ~Impl();
+      36             : 
+      37             :   void start();
+      38             :   void stop();
+      39             :   void setPeriod(const ros::Duration& duration, const bool reset = true);
+      40             :   void setCallback(const std::function<void(const ros::TimerEvent&)>& callback);
+      41             : 
+      42             :   friend class ThreadTimer;
+      43             : 
+      44             :   // to keep rule of five since we have a custom destructor
+      45             :   Impl(const Impl&) = delete;
+      46             :   Impl(Impl&&) = delete;
+      47             :   Impl& operator=(const Impl&) = delete;
+      48             :   Impl& operator=(Impl&&) = delete;
+      49             : 
+      50             : private:
+      51             :   std::thread thread_;
+      52             :   std::function<void(const ros::TimerEvent&)> callback_;
+      53             : 
+      54             :   bool oneshot_;
+      55             : 
+      56             :   bool breakableSleep(const ros::Time& until);
+      57             :   void threadFcn();
+      58             : 
+      59             :   std::mutex mutex_wakeup_;
+      60             :   std::condition_variable wakeup_cond_;
+      61             :   std::recursive_mutex mutex_state_;
+      62             :   bool running_;
+      63             :   ros::Duration delay_dur_;
+      64             :   bool ending_;
+      65             :   ros::Time next_expected_;
+      66             :   ros::Time last_expected_;
+      67             :   ros::Time last_real_;
+      68             : 
+      69             : };
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* ThreadTimer constructors and destructors//{ */
+      74             : 
+      75             : template <class ObjectType>
+      76           8 : ThreadTimer::ThreadTimer([[maybe_unused]] const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&),
+      77             :                          ObjectType* const obj, const bool oneshot, const bool autostart)
+      78           8 :   : ThreadTimer(nh, rate.expectedCycleTime(), callback, obj, oneshot, autostart)
+      79             : {
+      80           8 : }
+      81             : 
+      82             : template <class ObjectType>
+      83           8 : ThreadTimer::ThreadTimer([[maybe_unused]] const ros::NodeHandle& nh, const ros::Duration& duration,
+      84           8 :                          void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj, bool oneshot, const bool autostart) 
+      85             : {
+      86           8 :   const auto cbk = std::bind(callback, obj, std::placeholders::_1);
+      87           8 :   if (duration == ros::Duration(0))
+      88           0 :     oneshot = true;
+      89           8 :   this->impl_ = std::make_unique<Impl>(cbk, duration, oneshot);
+      90           8 :   if (autostart)
+      91           0 :     this->impl_->start();
+      92           8 : }
+      93             : 
+      94             : //}
+      95             : 
+      96             : #endif  // MRS_TIMER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.overview.html new file mode 100644 index 0000000000..fcb66ffefa --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.overview.html @@ -0,0 +1,44 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5eaff0c12ba0f0ee0c40adf9de476fcd7273979a GIT binary patch literal 532 zcmV+v0_**WP)WYG+H!gJy;wixEWkN+2HI)NNz7^h?e(6ei$EBmECa$YZdPbo z&&?9qN!l`6XNE9?C@%|f+=+O+hxoa^dC8|ovlXdJZ8P5Mb^}htY{=^O2l4r0f>@&z zs8-G-GtlRs7WS8Q4Naj!VYlL#*dr>O}oC~oA&GPcoS+nZ3=NZ9cU>_BRGE{}RklLXi&dpW;c zBPdq`J%+YgIsodmKUOYNAghxH_$}Pe8ijGBtP>l>ErpzMWuQ`Sl)LSv@r6H4l4sjS zx9^Ei-P+t!q)t6)>`2qi&SnbFW}=WM6nrvUXlj^Fio7N%@i_Cs-1BmH0tK`Cf-@K| zG8cLl(@=qVKn&f+P2+K#=lS`>-!T-NNCQh?%^=ZVsQ0Nei5g|Z{u?uHY__wee90dK WkIr@^1TfP80000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-04-25 05:42:57Functions:424887.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::doTransform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
geometry_msgs::Quaternion_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Point_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
cv::Point3_<double> mrs_lib::Transformer::copyChangeFrame<cv::Point3_<double> >(cv::Point3_<double> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
Eigen::Quaternion<double, 0> mrs_lib::Transformer::copyChangeFrame<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<cv::Point3_<double> > mrs_lib::Transformer::doTransform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transformImpl<cv::Point3_<double> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, cv::Point3_<double> const&)1
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transformImpl<Eigen::Quaternion<double, 0> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, Eigen::Quaternion<double, 0> const&)2
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)2
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)4
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)5
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)6
std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > mrs_lib::Transformer::transform<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Point_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)18
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Vector3_<std::allocator<void> > const&)18
geometry_msgs::Vector3Stamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)89
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)89
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)720
std::optional<mrs_msgs::ReferenceStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)21790
std::optional<mrs_msgs::ReferenceStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)21790
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)21790
geometry_msgs::PointStamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)23503
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)23503
std::optional<mrs_msgs::ReferenceStamped_<std::allocator<void> > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)90976
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)96400
geometry_msgs::PoseStamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)156504
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)156504
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)215548
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)238753
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)238982
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::PointStamped_<std::allocator<void> > const&)239051
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)262532
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)576185
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PoseStamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)576547
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)619336
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)732898
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)739595
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)739637
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)739804
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)740362
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)740451
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&)775838
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html new file mode 100644 index 0000000000..ee4de70c11 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html @@ -0,0 +1,272 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-04-25 05:42:57Functions:424887.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)619336
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)215548
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)740362
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)18
std::optional<cv::Point3_<double> > mrs_lib::Transformer::doTransform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::doTransform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)6
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&)775838
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::PointStamped_<std::allocator<void> > const&)239051
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)740451
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Point_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Vector3_<std::allocator<void> > const&)18
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transformImpl<cv::Point3_<double> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, cv::Point3_<double> const&)1
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transformImpl<Eigen::Quaternion<double, 0> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, Eigen::Quaternion<double, 0> const&)2
geometry_msgs::Quaternion_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::PoseStamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)156504
geometry_msgs::PointStamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)23503
geometry_msgs::Vector3Stamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)89
geometry_msgs::Point_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
cv::Point3_<double> mrs_lib::Transformer::copyChangeFrame<cv::Point3_<double> >(cv::Point3_<double> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
Eigen::Quaternion<double, 0> mrs_lib::Transformer::copyChangeFrame<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PoseStamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)576547
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)576185
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)238753
std::optional<geometry_msgs::PointStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)238982
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)739595
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)739637
std::optional<mrs_msgs::ReferenceStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)21790
std::optional<mrs_msgs::ReferenceStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)21790
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)732898
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)262532
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)739804
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)21790
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)156504
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)23503
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)89
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)5
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)96400
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)720
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)4
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)2
std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > mrs_lib::Transformer::transform<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<mrs_msgs::ReferenceStamped_<std::allocator<void> > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)90976
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.frameset.html new file mode 100644 index 0000000000..493a6837a5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html new file mode 100644 index 0000000000..13227a858f --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html @@ -0,0 +1,280 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-04-25 05:42:57Functions:424887.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef TRANSFORMER_HPP
+       2             : #define TRANSFORMER_HPP
+       3             : 
+       4             : // clang: MatousFormat
+       5             : 
+       6             : namespace mrs_lib
+       7             : {
+       8             : 
+       9             :   // | --------------------- helper methods --------------------- |
+      10             : 
+      11             :   /* getHeader() overloads for different message types (pointers, pointclouds etc) //{ */
+      12             : 
+      13             :   template <typename msg_t>
+      14     1757024 :   std_msgs::Header Transformer::getHeader(const msg_t& msg)
+      15             :   {
+      16     1757024 :     return msg.header;
+      17             :   }
+      18             : 
+      19             :   template <typename pt_t>
+      20             :   std_msgs::Header Transformer::getHeader(const pcl::PointCloud<pt_t>& cloud)
+      21             :   {
+      22             :     std_msgs::Header ret;
+      23             :     pcl_conversions::fromPCL(cloud.header, ret);
+      24             :     return ret;
+      25             :   }
+      26             : 
+      27             :   //}
+      28             : 
+      29             :   /* setHeader() overloads for different message types (pointers, pointclouds etc) //{ */
+      30             : 
+      31             :   template <typename msg_t>
+      32      180096 :   void Transformer::setHeader(msg_t& msg, const std_msgs::Header& header)
+      33             :   {
+      34      180096 :     msg.header = header;
+      35      180096 :   }
+      36             : 
+      37             :   template <typename pt_t>
+      38             :   void Transformer::setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::Header& header)
+      39             :   {
+      40             :     pcl_conversions::toPCL(header, cloud.header);
+      41             :   }
+      42             : 
+      43             :   //}
+      44             : 
+      45             :   /* copyChangeFrame() helper function //{ */
+      46             : 
+      47             :   template <typename T>
+      48      180096 :   T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
+      49             :   {
+      50      180096 :     T ret = what;
+      51             :     if constexpr (has_header_member_v<T>)
+      52             :     {
+      53      360192 :       std_msgs::Header new_header = getHeader(what);
+      54      180096 :       new_header.frame_id = frame_id;
+      55      180096 :       setHeader(ret, new_header);
+      56             :     }
+      57      180096 :     return ret;
+      58             :   }
+      59             : 
+      60             :   //}
+      61             : 
+      62             :   /* transformImpl() //{ */
+      63             : 
+      64             :   template <class T>
+      65     1755383 :   std::optional<T> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const T& what)
+      66             :   {
+      67     3510746 :     const std::string from_frame = frame_from(tf);
+      68     3510723 :     const std::string to_frame = frame_to(tf);
+      69             : 
+      70     1755374 :     if (from_frame == to_frame)
+      71      180096 :       return copyChangeFrame(what, from_frame);
+      72             : 
+      73     3150560 :     const std::string latlon_frame_name = resolveFrameImpl(LATLON_ORIGIN);
+      74             : 
+      75             :     // First, check if the transformation is from/to the latlon frame
+      76             :     // if conversion between UVM and LatLon coordinates is defined for this message, it may be resolved
+      77             :     if constexpr (UTMLL_exists_v<Transformer, T>)
+      78             :     {
+      79             :       // check for transformation from LAT-LON GPS
+      80      834895 :       if (from_frame == latlon_frame_name)
+      81             :       {
+      82           2 :         const std::optional<T> tmp = LLtoUTM(what, getFramePrefix(from_frame));
+      83           2 :         if (!tmp.has_value())
+      84           0 :           return std::nullopt;
+      85           2 :         return doTransform(tmp.value(), tf);
+      86             :       }
+      87             :       // check for transformation to LAT-LON GPS
+      88      834895 :       else if (to_frame == latlon_frame_name)
+      89             :       {
+      90        4010 :         const std::optional<T> tmp = doTransform(what, tf);
+      91        2006 :         if (!tmp.has_value())
+      92           0 :           return std::nullopt;
+      93        2006 :         return UTMtoLL(tmp.value(), getFramePrefix(to_frame));
+      94             :       }
+      95             :     }
+      96             :     else
+      97             :     {
+      98             :       // by default, transformation from/to LATLON is undefined, so return nullopt if it's attempted
+      99      740388 :       if (from_frame == latlon_frame_name || to_frame == latlon_frame_name)
+     100             :       {
+     101           2 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[" << node_name_ << "]: Transformer: cannot transform message of this type (" << typeid(T).name() << ") to/from latitude/longitude coordinates!");
+     102           2 :         return std::nullopt;
+     103             :       }
+     104             :     }
+     105             : 
+     106             :     // otherwise it's just an ol' borin' transformation
+     107     1573278 :     return doTransform(what, tf);
+     108             :   }
+     109             : 
+     110             :   //}
+     111             : 
+     112             :   /* doTransform() //{ */
+     113             : 
+     114             :   template <class T>
+     115     1575287 :   std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::TransformStamped& tf)
+     116             :   {
+     117             :     try
+     118             :     {
+     119     3150481 :       T result;
+     120     1575269 :       tf2::doTransform(what, result, tf);
+     121     1575275 :       return result;
+     122             :     }
+     123           0 :     catch (tf2::TransformException& ex)
+     124             :     {
+     125           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: Transformer: Error during transform from \"%s\" frame to \"%s\" frame.\n\tMSG: %s", node_name_.c_str(), frame_from(tf).c_str(),
+     126             :                         frame_to(tf).c_str(), ex.what());
+     127           0 :       return std::nullopt;
+     128             :     }
+     129             :   }
+     130             : 
+     131             :   //}
+     132             : 
+     133             :   // | ------------------ user-callable methods ----------------- |
+     134             : 
+     135             :   /* transformSingle() //{ */
+     136             : 
+     137             :   template <class T>
+     138     1576594 :   std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
+     139             :   {
+     140     3154128 :     const std_msgs::Header orig_header = getHeader(what);
+     141     3153210 :     return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
+     142             :   }
+     143             : 
+     144             :   template <class T>
+     145     1576686 :   std::optional<T> Transformer::transformSingle(const std::string& from_frame_raw, const T& what, const std::string& to_frame_raw, const ros::Time& time_stamp)
+     146             :   {
+     147     3154228 :     std::scoped_lock lck(mutex_);
+     148             : 
+     149     1577559 :     if (!initialized_)
+     150             :     {
+     151           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform, not initialized", node_name_.c_str());
+     152           0 :       return std::nullopt;
+     153             :     }
+     154             : 
+     155     3155097 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     156     3155067 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     157     3155073 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     158             : 
+     159             :     // get the transform
+     160     3155096 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     161     1577560 :     if (!tf_opt.has_value())
+     162       10313 :       return std::nullopt;
+     163     1567223 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     164             : 
+     165             :     // do the transformation
+     166     3134454 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     167     1567237 :     return transformImpl(tf_resolved, what);
+     168             :   }
+     169             : 
+     170             :   //}
+     171             : 
+     172             :   /* transform() //{ */
+     173             : 
+     174             :   template <class T>
+     175      188114 :   std::optional<T> Transformer::transform(const T& what, const geometry_msgs::TransformStamped& tf)
+     176             :   {
+     177      376228 :     std::scoped_lock lck(mutex_);
+     178             : 
+     179      188114 :     if (!initialized_)
+     180             :     {
+     181           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform, not initialized", node_name_.c_str());
+     182           0 :       return std::nullopt;
+     183             :     }
+     184             : 
+     185      376228 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     186      376228 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     187      376228 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     188             : 
+     189      188114 :     return transformImpl(tf_resolved, what);
+     190             :   }
+     191             : 
+     192             :   /* //} */
+     193             : 
+     194             : }
+     195             : 
+     196             : #endif // TRANSFORMER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.overview.html new file mode 100644 index 0000000000..d41ec041c1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.overview.html @@ -0,0 +1,69 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..69b889343341273e8bef5883143f2c7f010d788c GIT binary patch literal 841 zcmV-P1GfB$P)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp5jOU|8 zm6*pKyIQ-hGyRjermXd?hXi>IwazyX3(i%hbuwvthBHOeonQ2N;&1Z0*vxv0FKkHf z5%(@%6Jkt)iC(8wK&8|RuaH^81_j-XL`XACb!)TCz-MUT0~8(uc7t z2&n1{l;yC$h944rBoyxje$Moz+gZ{cokX_i%-BB`h36LxRx~S?roGZWJS3vIIHF)1 z;7`%>dPNxQgNyBy<2XKtldUJgbY5^<-q4+?v7Bz=7KW} z;*t-ooK%pwg$jQ$H)ZJWbR!k~TIMUtUBXOE1poPh4x|Y981eCdGs5VjC|%HGuPM#B z({Ba7s{?qf0l3`(I5wIz%!wm8KJ<<>p6JG=b(RUHOAOQjOc9A4=~?sk1Id2u&9b*` z&GDAvhTuK`|4BFdc`5Ky*uML$5od*&GsiDE7edP;^WyApF2CD*Fkx>u>_x&}Y1jc3 zdH`E0)@E^$;F|us;lbnV7enyq;Ot6A?3{@zMRM)Im12h+hpXLJgEtBQRl4Uw5ESsn zxv)@cow+Ji%Ul_k*r5xjIAiKl9(osJPMT@V#Nk?#TC*Zu!75C{Vnn*v61_YmEFlW9 z|ICIPmBKc?`l{aSxz_A%H7i@Q|JCfDs%F#VqOZ*FW#NO_;^c)s6VB%2|26#qR3G;^ To_s9e00000NkvXXu0mjfg!h-E literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html new file mode 100644 index 0000000000..cccee6c5c6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-04-25 05:42:57Functions:81266.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UKF<4, 1, 2>::setConstants(double, double, double)0
mrs_lib::UKF<4, 1, 2>::setTransitionModel(std::function<Eigen::Matrix<double, 4, 1, 0, 4, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double)> const&)0
mrs_lib::UKF<4, 1, 2>::setObservationModel(std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&)> const&)0
mrs_lib::UKF<4, 1, 2>::UKF()0
mrs_lib::UKF<4, 1, 2>::computeWeights()1
mrs_lib::UKF<4, 1, 2>::UKF(std::function<Eigen::Matrix<double, 4, 1, 0, 4, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&)> const&, double, double, double)1
mrs_lib::UKF<4, 1, 2>::computeInverse(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const100
mrs_lib::UKF<4, 1, 2>::computeKalmanGain(Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 4, 2, 0, 4, 2> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const100
mrs_lib::UKF<4, 1, 2>::correct(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const100
mrs_lib::UKF<4, 1, 2>::predict(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double) const100
mrs_lib::UKF<4, 1, 2>::computePaSqrt(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&) const200
mrs_lib::UKF<4, 1, 2>::computeSigmas(Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&) const200
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html new file mode 100644 index 0000000000..51cdacd64d --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-04-25 05:42:57Functions:81266.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UKF<4, 1, 2>::setConstants(double, double, double)0
mrs_lib::UKF<4, 1, 2>::computeWeights()1
mrs_lib::UKF<4, 1, 2>::setTransitionModel(std::function<Eigen::Matrix<double, 4, 1, 0, 4, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double)> const&)0
mrs_lib::UKF<4, 1, 2>::setObservationModel(std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&)> const&)0
mrs_lib::UKF<4, 1, 2>::UKF(std::function<Eigen::Matrix<double, 4, 1, 0, 4, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (Eigen::Matrix<double, 4, 1, 0, 4, 1> const&)> const&, double, double, double)1
mrs_lib::UKF<4, 1, 2>::UKF()0
mrs_lib::UKF<4, 1, 2>::computePaSqrt(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&) const200
mrs_lib::UKF<4, 1, 2>::computeSigmas(Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&) const200
mrs_lib::UKF<4, 1, 2>::computeInverse(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const100
mrs_lib::UKF<4, 1, 2>::computeKalmanGain(Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 4, 2, 0, 4, 2> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const100
mrs_lib::UKF<4, 1, 2>::correct(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const100
mrs_lib::UKF<4, 1, 2>::predict(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double) const100
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.frameset.html new file mode 100644 index 0000000000..aac3d90c80 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html new file mode 100644 index 0000000000..05cf6a4df5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-04-25 05:42:57Functions:81266.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : 
+       3             : #ifndef UKF_HPP
+       4             : #define UKF_HPP
+       5             : 
+       6             : /**  \file
+       7             :      \brief Implements UKF - a class implementing the Unscented Kalman Filter.
+       8             :      \author Tomáš Báča - bacatoma@fel.cvut.cz (original implementation)
+       9             :      \author Matouš Vrba - vrbamato@fel.cvut.cz (rewrite, documentation)
+      10             :  */
+      11             : 
+      12             : #include <ros/ros.h>
+      13             : #include <mrs_lib/ukf.h>
+      14             : 
+      15             : namespace mrs_lib
+      16             : {
+      17             :   /* constructor //{ */
+      18             : 
+      19             :   template <int n_states, int n_inputs, int n_measurements>
+      20           0 :   UKF<n_states, n_inputs, n_measurements>::UKF()
+      21             :   {
+      22           0 :   }
+      23             : 
+      24             :   template <int n_states, int n_inputs, int n_measurements>
+      25           1 :   UKF<n_states, n_inputs, n_measurements>::UKF(const transition_model_t& transition_model, const observation_model_t& observation_model, const double alpha, const double kappa, const double beta)
+      26           1 :     : m_alpha(alpha), m_kappa(kappa), m_beta(beta), m_Wm(W_t::Zero()), m_Wc(W_t::Zero()), m_transition_model(transition_model), m_observation_model(observation_model)
+      27             :   {
+      28           1 :     assert(alpha > 0.0);
+      29           1 :     computeWeights();
+      30           1 :   }
+      31             : 
+      32             :   //}
+      33             : 
+      34             :   /* computeWeights() //{ */
+      35             : 
+      36             :   template <int n_states, int n_inputs, int n_measurements>
+      37           1 :   void UKF<n_states, n_inputs, n_measurements>::computeWeights()
+      38             :   {
+      39             :     // initialize lambda
+      40             :     /* m_lambda = double(n) * (m_alpha * m_alpha - 1.0); */
+      41           1 :     m_lambda = m_alpha*m_alpha*(double(n) + m_kappa) - double(n);
+      42             : 
+      43             :     // initialize first terms of the weights
+      44           1 :     m_Wm(0) = m_lambda / (double(n) + m_lambda);
+      45           1 :     m_Wc(0) = m_Wm(0) + (1.0 - m_alpha*m_alpha + m_beta);
+      46             : 
+      47             :     // initialize the rest of the weights
+      48           9 :     for (int i = 1; i < w; i++)
+      49             :     {
+      50           8 :       m_Wm(i) = (1.0 - m_Wm(0))/(w - 1.0);
+      51           8 :       m_Wc(i) = m_Wm(i);
+      52             :     }
+      53           1 :   }
+      54             : 
+      55             :   //}
+      56             : 
+      57             :   /* setConstants() //{ */
+      58             : 
+      59             :   template <int n_states, int n_inputs, int n_measurements>
+      60             :   // update the UKF constants
+      61           0 :   void UKF<n_states, n_inputs, n_measurements>::setConstants(const double alpha, const double kappa, const double beta)
+      62             :   {
+      63           0 :     m_alpha = alpha;
+      64           0 :     m_kappa = kappa;
+      65           0 :     m_beta  = beta;
+      66             : 
+      67           0 :     computeWeights();
+      68           0 :   }
+      69             : 
+      70             :   //}
+      71             : 
+      72             :     /* setTransitionModel() method //{ */
+      73             : 
+      74             :     template <int n_states, int n_inputs, int n_measurements>
+      75           0 :     void UKF<n_states, n_inputs, n_measurements>::setTransitionModel(const transition_model_t& transition_model)
+      76             :     {
+      77           0 :       m_transition_model = transition_model;
+      78           0 :     }
+      79             : 
+      80             :     //}
+      81             : 
+      82             :     /* setObservationModel() method //{ */
+      83             : 
+      84             :     template <int n_states, int n_inputs, int n_measurements>
+      85           0 :     void UKF<n_states, n_inputs, n_measurements>::setObservationModel(const observation_model_t& observation_model)
+      86             :     {
+      87           0 :       m_observation_model = observation_model;
+      88           0 :     }
+      89             : 
+      90             :     //}
+      91             : 
+      92             :   /* computePaSqrt() method //{ */
+      93             :   template <int n_states, int n_inputs, int n_measurements>
+      94         200 :   typename UKF<n_states, n_inputs, n_measurements>::P_t UKF<n_states, n_inputs, n_measurements>::computePaSqrt(const P_t& P) const
+      95             :   {
+      96             :     // calculate the square root of the covariance matrix
+      97         200 :     const P_t Pa = (double(n) + m_lambda)*P;
+      98             : 
+      99             :     /* Eigen::SelfAdjointEigenSolver<P_t> es(Pa); */
+     100         200 :     Eigen::LLT<P_t> llt(Pa);
+     101         200 :     if (llt.info() != Eigen::Success)
+     102             :     {
+     103           0 :       P_t tmp = Pa + (double(n) + m_lambda)*1e-9*P_t::Identity();
+     104           0 :       llt.compute(tmp);
+     105           0 :       if (llt.info() != Eigen::Success)
+     106             :       {
+     107           0 :         ROS_WARN("UKF: taking the square root of covariance during sigma point generation failed.");
+     108           0 :         throw square_root_exception();
+     109             :       }
+     110             :     }
+     111             : 
+     112         200 :     const P_t Pa_sqrt = llt.matrixL();
+     113         400 :     return Pa_sqrt;
+     114             :   }
+     115             :   //}
+     116             : 
+     117             :   /* computeInverse() method //{ */
+     118             :   template <int n_states, int n_inputs, int n_measurements>
+     119         100 :   typename UKF<n_states, n_inputs, n_measurements>::Pzz_t UKF<n_states, n_inputs, n_measurements>::computeInverse(const Pzz_t& Pzz) const
+     120             :   {
+     121         100 :     Eigen::ColPivHouseholderQR<Pzz_t> qr(Pzz);
+     122         100 :     if (!qr.isInvertible())
+     123             :     {
+     124             :       // add some stuff to the tmp matrix diagonal to make it invertible
+     125           0 :       Pzz_t tmp = Pzz + 1e-9*Pzz_t::Identity(Pzz.rows(), Pzz.cols());
+     126           0 :       qr.compute(tmp);
+     127           0 :       if (!qr.isInvertible())
+     128             :       {
+     129             :         // never managed to make this happen except for explicitly putting NaNs in the input
+     130           0 :         ROS_ERROR("UKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)");
+     131           0 :         throw inverse_exception();
+     132             :       }
+     133           0 :       ROS_WARN("UKF: artificially inflating matrix for inverse computation! Check your covariances (the measurement's might be too low...)");
+     134             :     }
+     135         100 :     Pzz_t ret = qr.inverse();
+     136         200 :     return ret;
+     137             :   }
+     138             :   //}
+     139             : 
+     140             :   /* computeKalmanGain() method //{ */
+     141             :   template <int n_states, int n_inputs, int n_measurements>
+     142         100 :   typename UKF<n_states, n_inputs, n_measurements>::K_t UKF<n_states, n_inputs, n_measurements>::computeKalmanGain([[maybe_unused]] const x_t& x, [[maybe_unused]] const z_t& inn, const K_t& Pxz, const Pzz_t& Pzz) const
+     143             :   {
+     144         100 :     const Pzz_t Pzz_inv = computeInverse(Pzz);
+     145         100 :     const K_t K = Pxz * Pzz_inv;
+     146         200 :     return K;
+     147             :   }
+     148             :   //}
+     149             : 
+     150             :   /* computeSigmas() method //{ */
+     151             :   template <int n_states, int n_inputs, int n_measurements>
+     152         200 :   typename UKF<n_states, n_inputs, n_measurements>::X_t UKF<n_states, n_inputs, n_measurements>::computeSigmas(const x_t& x, const P_t& P) const
+     153             :   {
+     154             :     // calculate sigma points
+     155             :     // fill in the middle of the elipsoid
+     156         200 :     X_t S;
+     157         200 :     S.col(0) = x;
+     158             : 
+     159         200 :     const P_t P_sqrt = computePaSqrt(P);
+     160         200 :     const auto xrep = x.replicate(1, n);
+     161             : 
+     162             :     // positive sigma points
+     163         200 :     S.template block<n, n>(0, 1) = xrep + P_sqrt;
+     164             : 
+     165             :     // negative sigma points
+     166         200 :     S.template block<n, n>(0, n+1) = xrep - P_sqrt;
+     167             : 
+     168             :     /* std::cout << "x: " << std::endl << x << std::endl; */
+     169             :     /* std::cout << "S rowmean: " << std::endl << S.rowwise().mean() << std::endl; */
+     170             : 
+     171         400 :     return S;
+     172             :   }
+     173             :   //}
+     174             : 
+     175             :   /* predict() method //{ */
+     176             : 
+     177             :   template <int n_states, int n_inputs, int n_measurements>
+     178         100 :   typename UKF<n_states, n_inputs, n_measurements>::statecov_t UKF<n_states, n_inputs, n_measurements>::predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const
+     179             :   {
+     180         100 :     const auto& x = sc.x;
+     181         100 :     const auto& P = sc.P;
+     182         100 :     statecov_t ret;
+     183             : 
+     184         100 :     const X_t S = computeSigmas(x, P);
+     185             : 
+     186             :     // propagate sigmas through the transition model
+     187         100 :     X_t X;
+     188        1000 :     for (int i = 0; i < w; i++)
+     189             :     {
+     190         900 :       X.col(i) = m_transition_model(S.col(i), u, dt);
+     191             :     }
+     192             : 
+     193             :     /* std::cout << "x: " << std::endl << x << std::endl; */
+     194             :     /* std::cout << "X rowmean: " << std::endl << X.rowwise().mean() << std::endl; */
+     195             :     /* std::cout << "m_Wm sum: " << m_Wm.sum() << std::endl; */
+     196             : 
+     197             :     // recompute the state vector
+     198         100 :     ret.x = x_t::Zero();
+     199        1000 :     for (int i = 0; i < w; i++)
+     200             :     {
+     201             :       //TODO: WHY DOES THIS SHIT WORK IF I SUBSTITUTE m_Wm(i) FOR 1.0/w ??
+     202         900 :       x_t tmp = 1.0/w * X.col(i);
+     203             :       /* x_t tmp = m_Wm(i) * X.col(i); */
+     204         900 :       ret.x += tmp;
+     205             :     }
+     206             : 
+     207             :     // recompute the covariance
+     208         100 :     ret.P = P_t::Zero();
+     209        1000 :     for (int i = 0; i < w; i++)
+     210             :     {
+     211         900 :       ret.P += m_Wc(i) * (X.col(i) - ret.x) * (X.col(i) - ret.x).transpose();
+     212             :     }
+     213         100 :     ret.P += Q;
+     214             : 
+     215         200 :     return ret;
+     216             :   }
+     217             : 
+     218             :   //}
+     219             : 
+     220             :   /* correct() method //{ */
+     221             : 
+     222             :   template <int n_states, int n_inputs, int n_measurements>
+     223         100 :   typename UKF<n_states, n_inputs, n_measurements>::statecov_t UKF<n_states, n_inputs, n_measurements>::correct(const statecov_t& sc, const z_t& z, const R_t& R) const
+     224             :   {
+     225         100 :     const auto& x = sc.x;
+     226         100 :     const auto& P = sc.P;
+     227         100 :     const X_t S = computeSigmas(x, P);
+     228             : 
+     229             :     // propagate sigmas through the observation model
+     230         100 :     Z_t Z_exp(z.rows(), w);
+     231        1000 :     for (int i = 0; i < w; i++)
+     232             :     {
+     233         900 :       Z_exp.col(i) = m_observation_model(S.col(i));
+     234             :     }
+     235             : 
+     236             :     // compute expected measurement
+     237         100 :     z_t z_exp = z_t::Zero(z.rows());
+     238        1000 :     for (int i = 0; i < w; i++)
+     239             :     {
+     240         900 :       z_exp += m_Wm(i) * Z_exp.col(i);
+     241             :     }
+     242             : 
+     243             :     // compute the covariance of measurement
+     244         100 :     Pzz_t Pzz = Pzz_t::Zero(z.rows(), z.rows());
+     245        1000 :     for (int i = 0; i < w; i++)
+     246             :     {
+     247         900 :       Pzz += m_Wc(i) * (Z_exp.col(i) - z_exp) * (Z_exp.col(i) - z_exp).transpose();
+     248             :     }
+     249         100 :     Pzz += R;
+     250             : 
+     251             :     // compute cross covariance
+     252         100 :     K_t Pxz = K_t::Zero(n, z.rows());
+     253        1000 :     for (int i = 0; i < w; i++)
+     254             :     {
+     255         900 :       Pxz += m_Wc(i) * (S.col(i) - x) * (Z_exp.col(i) - z_exp).transpose();
+     256             :     }
+     257             : 
+     258             :     // compute Kalman gain
+     259         100 :     const z_t inn = (z - z_exp); // innovation
+     260         100 :     const K_t K = computeKalmanGain(sc.x, inn, Pxz, Pzz);
+     261             : 
+     262             :     // check whether the inverse produced valid numbers
+     263         100 :     if (!K.array().isFinite().all())
+     264             :     {
+     265           0 :       ROS_ERROR("UKF: inverting of Pzz in correction update produced non-finite numbers!!! Fix your covariances (the measurement's is probably too low...)");
+     266           0 :       throw inverse_exception();
+     267             :     }
+     268             : 
+     269             :     // correct
+     270         100 :     statecov_t ret;
+     271         100 :     ret.x = x + K * inn;
+     272         100 :     ret.P = P - K * Pzz * K.transpose();
+     273         200 :     return ret;
+     274             :   }
+     275             : 
+     276             :   //}
+     277             : 
+     278             : }  // namespace mrs_lib
+     279             : 
+     280             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.overview.html new file mode 100644 index 0000000000..68c88bdc08 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.overview.html @@ -0,0 +1,90 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1d33d5b3039a981579bb4f50a9c00a2c8709ad9b GIT binary patch literal 1198 zcmV;f1X25mP)Y@{;Pb0B(89X1lp$WlRn3FWZf6Cw_WVJ+}+ydtQ`=_ zd4PPgbS9hXQz+oPx)ki&7 z-lQE#rEk)z9uGELd87|74GK~T#|W?+u3XZe6t2^vl^w1d^jri}M|c;c3rf)C56QZq zP>s|XTiZ;bsJ#Vv-hdqz_$BTdbHN)i=DQZ)l5v>`+*`m1a!FMX_`s(SmpO- zp$vyfR|C*SHhH6}0WwL`B(6KO!^))|7!eAY$hGhdq-(Utn_=63OFES_bj!U50C8YR z|N7J=_cr$>VLl@P%-P){#tAJ~{L6R_w;t2FXAF>2UxyPws4?ZRe#OISm*?A}%rGNq zZ^^7Vo9;iJkq`B_Y=@kl0KzY}Aw>xP^-|D;VL_|NIq#{C2ogQcHsOZP_(abshj)TrRs^3 zRDJ%qOg&hws;@$&LlQi$D@$mg=^NrpvL6ojS`Pb^=MJ& zI<$0_Mxku)5@`mY@u80@ds!ioK4>QJ-s>hI8lqjf%z4bxSXJQzPkg@O>F7ldYVaim ze6B)6>E3j9;8EqM3Hx9J`$AVSoejBw%?C7ptCNz#zfL;|HFrYu4`1RPbHQ`N;s5{u M07*qoM6N<$f)tiG*8l(j literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func-sort-c.html new file mode 100644 index 0000000000..25200aae13 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - vector_converter.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1818100.0 %
Date:2024-04-25 05:42:57Functions:1818100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::impl::convertTo<geometry_msgs::Vector3_<std::allocator<void> > >(double, double, double)8
cv::Vec<double, 3> mrs_lib::impl::convertTo<cv::Vec<double, 3> >(double, double, double)8
cv::Vec<float, 3> mrs_lib::impl::convertTo<cv::Vec<float, 3> >(double, double, double)8
pcl::PointXYZ mrs_lib::impl::convertTo<pcl::PointXYZ>(double, double, double)8
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::impl::convertTo<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(double, double, double)8
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::impl::convertTo<Eigen::Matrix<float, 3, 1, 0, 3, 1> >(double, double, double)8
std::enable_if<has_xmem_v<geometry_msgs::Vector3_<std::allocator<void> > >, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)14
std::enable_if<(has_squarebracket_operator_v<cv::Vec<double, 3> >)&&(!(has_xfun_v<cv::Vec<double, 3> >)), std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<cv::Vec<double, 3> >(cv::Vec<double, 3> const&)14
std::enable_if<(has_squarebracket_operator_v<cv::Vec<float, 3> >)&&(!(has_xfun_v<cv::Vec<float, 3> >)), std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<cv::Vec<float, 3> >(cv::Vec<float, 3> const&)14
std::enable_if<has_xmem_v<pcl::PointXYZ>, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<pcl::PointXYZ>(pcl::PointXYZ const&)14
std::enable_if<has_xfun_v<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)14
std::enable_if<has_xfun_v<Eigen::Matrix<float, 3, 1, 0, 3, 1> >, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)14
std::enable_if<(!(has_xyz_constructor_v<geometry_msgs::Vector3_<std::allocator<void> > >))&&(has_xmem_v<geometry_msgs::Vector3_<std::allocator<void> > >), void>::type mrs_lib::impl::convertTo<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> >&, double, double, double)14
std::enable_if<has_xyz_constructor_v<cv::Vec<double, 3> >, void>::type mrs_lib::impl::convertTo<cv::Vec<double, 3> >(cv::Vec<double, 3>&, double, double, double)14
std::enable_if<has_xyz_constructor_v<cv::Vec<float, 3> >, void>::type mrs_lib::impl::convertTo<cv::Vec<float, 3> >(cv::Vec<float, 3>&, double, double, double)14
std::enable_if<has_xyz_constructor_v<pcl::PointXYZ>, void>::type mrs_lib::impl::convertTo<pcl::PointXYZ>(pcl::PointXYZ&, double, double, double)14
std::enable_if<has_xyz_constructor_v<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, void>::type mrs_lib::impl::convertTo<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1>&, double, double, double)14
std::enable_if<has_xyz_constructor_v<Eigen::Matrix<float, 3, 1, 0, 3, 1> >, void>::type mrs_lib::impl::convertTo<Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1>&, double, double, double)14
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func.html b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func.html new file mode 100644 index 0000000000..ec258eb5ed --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - vector_converter.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1818100.0 %
Date:2024-04-25 05:42:57Functions:1818100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::enable_if<has_xmem_v<geometry_msgs::Vector3_<std::allocator<void> > >, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)14
std::enable_if<(has_squarebracket_operator_v<cv::Vec<double, 3> >)&&(!(has_xfun_v<cv::Vec<double, 3> >)), std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<cv::Vec<double, 3> >(cv::Vec<double, 3> const&)14
std::enable_if<(has_squarebracket_operator_v<cv::Vec<float, 3> >)&&(!(has_xfun_v<cv::Vec<float, 3> >)), std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<cv::Vec<float, 3> >(cv::Vec<float, 3> const&)14
std::enable_if<has_xmem_v<pcl::PointXYZ>, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<pcl::PointXYZ>(pcl::PointXYZ const&)14
std::enable_if<has_xfun_v<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)14
std::enable_if<has_xfun_v<Eigen::Matrix<float, 3, 1, 0, 3, 1> >, std::tuple<double, double, double> >::type mrs_lib::impl::convertFrom<Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)14
std::enable_if<(!(has_xyz_constructor_v<geometry_msgs::Vector3_<std::allocator<void> > >))&&(has_xmem_v<geometry_msgs::Vector3_<std::allocator<void> > >), void>::type mrs_lib::impl::convertTo<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> >&, double, double, double)14
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::impl::convertTo<geometry_msgs::Vector3_<std::allocator<void> > >(double, double, double)8
std::enable_if<has_xyz_constructor_v<cv::Vec<double, 3> >, void>::type mrs_lib::impl::convertTo<cv::Vec<double, 3> >(cv::Vec<double, 3>&, double, double, double)14
cv::Vec<double, 3> mrs_lib::impl::convertTo<cv::Vec<double, 3> >(double, double, double)8
std::enable_if<has_xyz_constructor_v<cv::Vec<float, 3> >, void>::type mrs_lib::impl::convertTo<cv::Vec<float, 3> >(cv::Vec<float, 3>&, double, double, double)14
cv::Vec<float, 3> mrs_lib::impl::convertTo<cv::Vec<float, 3> >(double, double, double)8
std::enable_if<has_xyz_constructor_v<pcl::PointXYZ>, void>::type mrs_lib::impl::convertTo<pcl::PointXYZ>(pcl::PointXYZ&, double, double, double)14
pcl::PointXYZ mrs_lib::impl::convertTo<pcl::PointXYZ>(double, double, double)8
std::enable_if<has_xyz_constructor_v<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, void>::type mrs_lib::impl::convertTo<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1>&, double, double, double)14
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::impl::convertTo<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(double, double, double)8
std::enable_if<has_xyz_constructor_v<Eigen::Matrix<float, 3, 1, 0, 3, 1> >, void>::type mrs_lib::impl::convertTo<Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1>&, double, double, double)14
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::impl::convertTo<Eigen::Matrix<float, 3, 1, 0, 3, 1> >(double, double, double)8
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.frameset.html b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.frameset.html new file mode 100644 index 0000000000..bc2d20363d --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.html b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.html new file mode 100644 index 0000000000..0ffb91e075 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.html @@ -0,0 +1,173 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - vector_converter.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1818100.0 %
Date:2024-04-25 05:42:57Functions:1818100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file vector_converter.hpp
+       2             :      \brief Implements the convertTo() and convertFrom() functions for conversion between different vector representations (Eigen, OpenCV, tf2 etc.).
+       3             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       4             :  */
+       5             : #ifndef VECTOR_CONVERTER_HPP
+       6             : #define VECTOR_CONVERTER_HPP
+       7             : 
+       8             : #include <experimental/type_traits>
+       9             : 
+      10             : namespace mrs_lib
+      11             : {
+      12             :   namespace impl
+      13             :   {
+      14             :     using unw_t = std::tuple<double, double, double>;
+      15             : 
+      16             :     /* SFINAE magic - only for black wizards! //{ */
+      17             :     
+      18             :     #define GENERATE_HAS_MEMBER_FUNC(func, rettype)                                \
+      19             :     template<class T> using _has_##func##fun_chk =                                 \
+      20             :           decltype(std::declval<T &>().func());                                    \
+      21             :     template<class T> constexpr bool has_##func##fun_v =                           \
+      22             :           std::experimental::is_detected_convertible_v<rettype, _has_##func##fun_chk, T>;
+      23             :     
+      24             :     #define GENERATE_HAS_MEMBER(memb, type)                                        \
+      25             :     template<class T> using _has_##memb##mem_chk =                                 \
+      26             :           decltype(std::declval<T &>().memb);                                      \
+      27             :     template<class T> constexpr bool has_##memb##mem_v =                           \
+      28             :           std::experimental::is_detected_convertible_v<type, _has_##memb##mem_chk, T>;
+      29             :     
+      30             :     GENERATE_HAS_MEMBER_FUNC(x, double)
+      31             :     GENERATE_HAS_MEMBER(x, double)
+      32             : 
+      33             :     template<class T> using _has_squarebracket_operator_chk = decltype(std::declval<T &>()[0]);
+      34             :     template<class T> constexpr bool has_squarebracket_operator_v = std::experimental::is_detected_convertible_v<double, _has_squarebracket_operator_chk, T>;
+      35             : 
+      36             :     template<class T> constexpr bool has_xyz_constructor_v = std::experimental::is_constructible_v<T, double, double, double>;
+      37             :     
+      38             :     //}
+      39             : 
+      40             :     // convertFrom specialization for Eigen types
+      41             :     template <typename in_t>
+      42          28 :     std::enable_if_t<has_xfun_v<in_t>, unw_t> convertFrom(const in_t& in)
+      43             :     {
+      44          28 :       return {in.x(), in.y(), in.z()};
+      45             :     }
+      46             : 
+      47             :     // convertFrom specialization for plain member types
+      48             :     template <typename in_t>
+      49          28 :     std::enable_if_t<has_xmem_v<in_t>, unw_t> convertFrom(const in_t& in)
+      50             :     {
+      51          28 :       return {in.x, in.y, in.z};
+      52             :     }
+      53             : 
+      54             :     // convertFrom specialization for OpenCV vectors
+      55             :     template <typename in_t>
+      56          28 :     std::enable_if_t<has_squarebracket_operator_v<in_t> && !has_xfun_v<in_t>, unw_t> convertFrom(const in_t& in)
+      57             :     {
+      58          28 :       return {in[0], in[1], in[2]};
+      59             :     }
+      60             : 
+      61             :     // convertTo specialization for types with a constructor that takes three doubles
+      62             :     template <typename ret_t>
+      63          70 :     std::enable_if_t<has_xyz_constructor_v<ret_t>, void> convertTo(ret_t& out, const double x, const double y, const double z)
+      64             :     {
+      65          70 :       out = {x, y, z};
+      66          70 :     }
+      67             : 
+      68             :     // convertTo specialization for other types
+      69             :     template <typename ret_t>
+      70          14 :     std::enable_if_t<!has_xyz_constructor_v<ret_t> && has_xmem_v<ret_t>, void> convertTo(ret_t& out, const double x, const double y, const double z)
+      71             :     {
+      72          14 :       out.x = x;
+      73          14 :       out.y = y;
+      74          14 :       out.z = z;
+      75          14 :     }
+      76             : 
+      77             :     template <typename ret_t>
+      78          48 :     ret_t convertTo(const double x, const double y, const double z)
+      79             :     {
+      80          48 :       ret_t ret;
+      81          48 :       convertTo(ret, x, y, z);
+      82          48 :       return ret;
+      83             :     }
+      84             : 
+      85             :   }  // namespace impl
+      86             : 
+      87             : }  // namespace mrs_lib
+      88             : 
+      89             : #endif // VECTOR_CONVERTER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.overview.html new file mode 100644 index 0000000000..63bef275db --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.overview.html @@ -0,0 +1,43 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6df626b77730d3afc2366a6bff779a75aafec4bb GIT binary patch literal 456 zcmV;(0XP1MP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpT=9C9wI= zG+~i#a`WyY{t1@_iX){|^BF%e2Apb;YSze^m8gMB%2L!-%15Hk-82~W)kUrli=9UH zXfqZ%Mo03Ets&0^JGphqs3vt>etz8;@hnKG0j?worJ61qvzKP~-QnCFmovtA;4$>g z15G3eEP;sw3K^H>sc7)MJTCGyfPayv-naAeWNB=k&Qwp*nOXOuJVo&+)GW0cK6?<` zXmM5^yOTOTMmr{tb3xo5P|hd=GV*Ze^^!a;Plx3>^!Wt%g5Yu;Q8Aa~yN(k+hNUtf y=B!bZ)Ym-+-so$f6MbNjjPR)go?bgT?-)N0N#(4bA-TN(0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-25 05:42:57Functions:843135162.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
<unnamed>65.5 %57 / 8732.9 %28 / 85
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
<unnamed>33.3 %2 / 633.3 %1 / 3
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
<unnamed>33.3 %33 / 9934.1 %15 / 44
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
<unnamed>90.1 %100 / 11145.5 %20 / 44
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.6 %390 / 741
<unnamed>89.8 %44 / 4952.6 %390 / 741
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
<unnamed>81.5 %44 / 5453.2 %25 / 47
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
<unnamed>85.2 %23 / 2781.8 %9 / 11
mutex.h +
100.0%
+ 		100.0 %11 / 1183.8 %67 / 80
<unnamed>100.0 %11 / 1183.8 %67 / 80
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
<unnamed>100.0 %4 / 499.2 %118 / 119
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
<unnamed>100.0 %4 / 4-0 / 0
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
<unnamed>76.5 %104 / 136100.0 %4 / 4
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
<unnamed>100.0 %15 / 15100.0 %5 / 5
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %24 / 24
<unnamed>100.0 %3 / 3100.0 %24 / 24
vector_converter.h +
100.0%
+ 		100.0 %5 / 5100.0 %36 / 36
<unnamed>100.0 %5 / 5100.0 %36 / 36
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
<unnamed>90.3 %271 / 300100.0 %78 / 78
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/index-detail-sort-l.html b/mrs_lib/include/mrs_lib/index-detail-sort-l.html new file mode 100644 index 0000000000..57b9b67769 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-l.html @@ -0,0 +1,444 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-25 05:42:57Functions:843135162.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
<unnamed>33.3 %2 / 633.3 %1 / 3
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
<unnamed>33.3 %33 / 9934.1 %15 / 44
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
<unnamed>65.5 %57 / 8732.9 %28 / 85
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
<unnamed>76.5 %104 / 136100.0 %4 / 4
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
<unnamed>81.5 %44 / 5453.2 %25 / 47
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
<unnamed>85.2 %23 / 2781.8 %9 / 11
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.6 %390 / 741
<unnamed>89.8 %44 / 4952.6 %390 / 741
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
<unnamed>90.1 %100 / 11145.5 %20 / 44
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
<unnamed>90.3 %271 / 300100.0 %78 / 78
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %24 / 24
<unnamed>100.0 %3 / 3100.0 %24 / 24
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
<unnamed>100.0 %4 / 4-0 / 0
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
<unnamed>100.0 %4 / 499.2 %118 / 119
vector_converter.h +
100.0%
+ 		100.0 %5 / 5100.0 %36 / 36
<unnamed>100.0 %5 / 5100.0 %36 / 36
mutex.h +
100.0%
+ 		100.0 %11 / 1183.8 %67 / 80
<unnamed>100.0 %11 / 1183.8 %67 / 80
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
<unnamed>100.0 %15 / 15100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/index-detail.html b/mrs_lib/include/mrs_lib/index-detail.html new file mode 100644 index 0000000000..d4615149c9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail.html @@ -0,0 +1,444 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-25 05:42:57Functions:843135162.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
<unnamed>85.2 %23 / 2781.8 %9 / 11
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
<unnamed>65.5 %57 / 8732.9 %28 / 85
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
<unnamed>76.5 %104 / 136100.0 %4 / 4
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
<unnamed>81.5 %44 / 5453.2 %25 / 47
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
<unnamed>33.3 %33 / 9934.1 %15 / 44
mutex.h +
100.0%
+ 		100.0 %11 / 1183.8 %67 / 80
<unnamed>100.0 %11 / 1183.8 %67 / 80
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
<unnamed>90.3 %271 / 300100.0 %78 / 78
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
<unnamed>100.0 %4 / 499.2 %118 / 119
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
<unnamed>90.1 %100 / 11145.5 %20 / 44
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
<unnamed>33.3 %2 / 633.3 %1 / 3
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %24 / 24
<unnamed>100.0 %3 / 3100.0 %24 / 24
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.6 %390 / 741
<unnamed>89.8 %44 / 4952.6 %390 / 741
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
<unnamed>100.0 %4 / 4-0 / 0
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
<unnamed>100.0 %15 / 15100.0 %5 / 5
vector_converter.h +
100.0%
+ 		100.0 %5 / 5100.0 %36 / 36
<unnamed>100.0 %5 / 5100.0 %36 / 36
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/index-sort-f.html b/mrs_lib/include/mrs_lib/index-sort-f.html new file mode 100644 index 0000000000..0f5a8c0961 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-f.html @@ -0,0 +1,292 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-25 05:42:57Functions:843135162.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.6 %390 / 741
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
mutex.h +
100.0%
+ 		100.0 %11 / 1183.8 %67 / 80
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %24 / 24
vector_converter.h +
100.0%
+ 		100.0 %5 / 5100.0 %36 / 36
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/index-sort-l.html b/mrs_lib/include/mrs_lib/index-sort-l.html new file mode 100644 index 0000000000..55b2ded5e4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-l.html @@ -0,0 +1,292 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-25 05:42:57Functions:843135162.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.6 %390 / 741
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %24 / 24
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
vector_converter.h +
100.0%
+ 		100.0 %5 / 5100.0 %36 / 36
mutex.h +
100.0%
+ 		100.0 %11 / 1183.8 %67 / 80
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/index.html b/mrs_lib/include/mrs_lib/index.html new file mode 100644 index 0000000000..b854be0e2a --- /dev/null +++ b/mrs_lib/include/mrs_lib/index.html @@ -0,0 +1,292 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-25 05:42:57Functions:843135162.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
mutex.h +
100.0%
+ 		100.0 %11 / 1183.8 %67 / 80
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %24 / 24
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.6 %390 / 741
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
vector_converter.h +
100.0%
+ 		100.0 %5 / 5100.0 %36 / 36
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html new file mode 100644 index 0000000000..87f41e314a --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-25 05:42:57Functions:254753.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::varstepLKF<3, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 3, 3, 0, 3, 3> (double)> const&, std::function<Eigen::Matrix<double, 3, 1, 0, 3, 1> (double)> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)0
mrs_lib::varstepLKF<6, 2, 2>::varstepLKF(std::function<Eigen::Matrix<double, 6, 6, 0, 6, 6> (double)> const&, std::function<Eigen::Matrix<double, 6, 2, 0, 6, 2> (double)> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)0
mrs_lib::LKF<2, 1, 1>::LKF(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)0
mrs_lib::LKF<3, 1, 1>::LKF()0
std::enable_if<(1)!=(0), Eigen::Matrix<double, 4, 1, 0, 4, 1> >::type mrs_lib::LKF<4, 1, 2>::state_predict<1>(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)0
mrs_lib::LKF<4, 1, 2>::covariance_predict(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double)0
mrs_lib::LKF<4, 1, 2>::correction_optimized(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)0
mrs_lib::LKF<4, 1, 2>::LKF(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::LKF()0
mrs_lib::LKF<6, 2, 2>::LKF()0
mrs_lib::varstepLKF<3, 1, 1>::predict(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double) const0
mrs_lib::varstepLKF<6, 2, 2>::predict(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double) const0
mrs_lib::LKF<2, 1, 1>::inverse_exception::what() const0
mrs_lib::LKF<2, 1, 1>::predict(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double) const0
mrs_lib::LKF<3, 1, 1>::inverse_exception::what() const0
std::enable_if<(4)>=(0), mrs_lib::KalmanFilter<4, 1, 2>::statecov_t>::type mrs_lib::LKF<4, 1, 2>::correction_impl<4>(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::computeKalmanGain(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::inverse_exception::what() const0
mrs_lib::LKF<4, 1, 2>::correct(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const0
mrs_lib::LKF<4, 1, 2>::predict(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double) const0
mrs_lib::LKF<6, 2, 2>::inverse_exception::what() const0
mrs_lib::varstepLKF<2, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 2, 2, 0, 2, 2> (double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (double)> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)3
mrs_lib::LKF<2, 1, 1>::LKF()3
mrs_lib::LKF<6, 2, 2>::LKF(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 2, 0, 6, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)99
mrs_lib::LKF<3, 1, 1>::LKF(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)172
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5838
std::enable_if<(2)>=(0), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::LKF<2, 1, 1>::correction_impl<2>(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&) const5838
mrs_lib::LKF<2, 1, 1>::computeKalmanGain(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&) const5838
mrs_lib::LKF<2, 1, 1>::correct(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&) const5838
std::enable_if<(1)!=(0), Eigen::Matrix<double, 2, 1, 0, 2, 1> >::type mrs_lib::LKF<2, 1, 1>::state_predict<1>(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)17141
mrs_lib::LKF<2, 1, 1>::covariance_predict(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double)17141
mrs_lib::varstepLKF<2, 1, 1>::predict(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double) const17141
mrs_lib::LKF<6, 2, 2>::predict(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double) const172039
std::enable_if<(2)!=(0), Eigen::Matrix<double, 6, 1, 0, 6, 1> >::type mrs_lib::LKF<6, 2, 2>::state_predict<2>(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 2, 0, 6, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)172057
mrs_lib::LKF<6, 2, 2>::covariance_predict(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double)172068
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)198185
mrs_lib::LKF<6, 2, 2>::correct(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const198349
std::enable_if<(6)>=(0), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::LKF<6, 2, 2>::correction_impl<6>(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&) const198400
mrs_lib::LKF<6, 2, 2>::computeKalmanGain(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&) const198440
mrs_lib::LKF<3, 1, 1>::predict(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double) const297626
std::enable_if<(1)!=(0), Eigen::Matrix<double, 3, 1, 0, 3, 1> >::type mrs_lib::LKF<3, 1, 1>::state_predict<1>(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)297705
mrs_lib::LKF<3, 1, 1>::covariance_predict(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double)297795
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)453697
mrs_lib::LKF<3, 1, 1>::computeKalmanGain(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&) const454020
std::enable_if<(3)>=(0), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::LKF<3, 1, 1>::correction_impl<3>(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&) const454994
mrs_lib::LKF<3, 1, 1>::correct(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&) const455003
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/lkf.h.func.html b/mrs_lib/include/mrs_lib/lkf.h.func.html new file mode 100644 index 0000000000..24fe514f81 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-25 05:42:57Functions:254753.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::varstepLKF<2, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 2, 2, 0, 2, 2> (double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (double)> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)3
mrs_lib::varstepLKF<3, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 3, 3, 0, 3, 3> (double)> const&, std::function<Eigen::Matrix<double, 3, 1, 0, 3, 1> (double)> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)0
mrs_lib::varstepLKF<6, 2, 2>::varstepLKF(std::function<Eigen::Matrix<double, 6, 6, 0, 6, 6> (double)> const&, std::function<Eigen::Matrix<double, 6, 2, 0, 6, 2> (double)> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)0
std::enable_if<(1)!=(0), Eigen::Matrix<double, 2, 1, 0, 2, 1> >::type mrs_lib::LKF<2, 1, 1>::state_predict<1>(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)17141
mrs_lib::LKF<2, 1, 1>::covariance_predict(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double)17141
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5838
mrs_lib::LKF<2, 1, 1>::LKF(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)0
mrs_lib::LKF<2, 1, 1>::LKF()3
std::enable_if<(1)!=(0), Eigen::Matrix<double, 3, 1, 0, 3, 1> >::type mrs_lib::LKF<3, 1, 1>::state_predict<1>(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)297705
mrs_lib::LKF<3, 1, 1>::covariance_predict(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double)297795
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)453697
mrs_lib::LKF<3, 1, 1>::LKF(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)172
mrs_lib::LKF<3, 1, 1>::LKF()0
std::enable_if<(1)!=(0), Eigen::Matrix<double, 4, 1, 0, 4, 1> >::type mrs_lib::LKF<4, 1, 2>::state_predict<1>(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)0
mrs_lib::LKF<4, 1, 2>::covariance_predict(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double)0
mrs_lib::LKF<4, 1, 2>::correction_optimized(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)0
mrs_lib::LKF<4, 1, 2>::LKF(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::LKF()0
std::enable_if<(2)!=(0), Eigen::Matrix<double, 6, 1, 0, 6, 1> >::type mrs_lib::LKF<6, 2, 2>::state_predict<2>(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 2, 0, 6, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)172057
mrs_lib::LKF<6, 2, 2>::covariance_predict(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double)172068
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)198185
mrs_lib::LKF<6, 2, 2>::LKF(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 2, 0, 6, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)99
mrs_lib::LKF<6, 2, 2>::LKF()0
mrs_lib::varstepLKF<2, 1, 1>::predict(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double) const17141
mrs_lib::varstepLKF<3, 1, 1>::predict(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double) const0
mrs_lib::varstepLKF<6, 2, 2>::predict(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double) const0
std::enable_if<(2)>=(0), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::LKF<2, 1, 1>::correction_impl<2>(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&) const5838
mrs_lib::LKF<2, 1, 1>::computeKalmanGain(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&) const5838
mrs_lib::LKF<2, 1, 1>::inverse_exception::what() const0
mrs_lib::LKF<2, 1, 1>::correct(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&) const5838
mrs_lib::LKF<2, 1, 1>::predict(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double) const0
std::enable_if<(3)>=(0), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::LKF<3, 1, 1>::correction_impl<3>(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&) const454994
mrs_lib::LKF<3, 1, 1>::computeKalmanGain(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&) const454020
mrs_lib::LKF<3, 1, 1>::inverse_exception::what() const0
mrs_lib::LKF<3, 1, 1>::correct(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&) const455003
mrs_lib::LKF<3, 1, 1>::predict(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double) const297626
std::enable_if<(4)>=(0), mrs_lib::KalmanFilter<4, 1, 2>::statecov_t>::type mrs_lib::LKF<4, 1, 2>::correction_impl<4>(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::computeKalmanGain(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::inverse_exception::what() const0
mrs_lib::LKF<4, 1, 2>::correct(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const0
mrs_lib::LKF<4, 1, 2>::predict(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double) const0
std::enable_if<(6)>=(0), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::LKF<6, 2, 2>::correction_impl<6>(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&) const198400
mrs_lib::LKF<6, 2, 2>::computeKalmanGain(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&) const198440
mrs_lib::LKF<6, 2, 2>::inverse_exception::what() const0
mrs_lib::LKF<6, 2, 2>::correct(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const198349
mrs_lib::LKF<6, 2, 2>::predict(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double) const172039
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/lkf.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/lkf.h.gcov.frameset.html new file mode 100644 index 0000000000..60fa6745b7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/lkf.h.gcov.html b/mrs_lib/include/mrs_lib/lkf.h.gcov.html new file mode 100644 index 0000000000..9a1e40b036 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.gcov.html @@ -0,0 +1,460 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-25 05:42:57Functions:254753.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file LKF
+       3             :      \brief Defines LKF - a class, implementing the Linear Kalman Filter \cite LKF, as well as a few specialized variants.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : #ifndef LKFSYSTEMMODELS_H
+       7             : #define LKFSYSTEMMODELS_H
+       8             : 
+       9             : #include <mrs_lib/kalman_filter.h>
+      10             : #include <iostream>
+      11             : 
+      12             : namespace mrs_lib
+      13             : {
+      14             : 
+      15             :   /* class LKF //{ */
+      16             :   /**
+      17             :   * \brief Implementation of the Linear Kalman filter \cite LKF.
+      18             :   *
+      19             :   * The Linear Kalman filter (abbreviated LKF, \cite LKF) may be used for state filtration or estimation of linear
+      20             :   * stochastic discrete systems. It assumes that noise variables are sampled from multivariate gaussian distributions
+      21             :   * and takes into account apriori known parameters of these distributions (namely zero means and covariance matrices,
+      22             :   * which have to be specified by the user and are tunable parameters).
+      23             :   *
+      24             :   * The LKF C++ class itself is templated. This has its advantages and disadvantages. Main disadvantage is that it
+      25             :   * may be harder to use if you're not familiar with C++ templates, which, admittedly, can get somewhat messy,
+      26             :   * espetially during compilation. Another disadvantage is that if used unwisely, the compilation times can get
+      27             :   * much higher when using templates. The main advantage is compile-time checking (if it compiles, then it has
+      28             :   * a lower chance of crashing at runtime) and enabling more effective optimalizations during compilation. Also in case
+      29             :   * of Eigen, the code is arguably more readable when you use aliases to the specific Matrix instances instead of
+      30             :   * having Eigen::MatrixXd and Eigen::VectorXd everywhere.
+      31             :   *
+      32             :   * \tparam n_states         number of states of the system (length of the \f$ \mathbf{x} \f$ vector).
+      33             :   * \tparam n_inputs         number of inputs of the system (length of the \f$ \mathbf{u} \f$ vector).
+      34             :   * \tparam n_measurements   number of measurements of the system (length of the \f$ \mathbf{z} \f$ vector).
+      35             :   *
+      36             :   */
+      37             :   template <int n_states, int n_inputs, int n_measurements>
+      38             :   class LKF : public KalmanFilter<n_states, n_inputs, n_measurements>
+      39             :   {
+      40             :   public:
+      41             :     /* LKF definitions (typedefs, constants etc) //{ */
+      42             :     static constexpr int n = n_states;            /*!< \brief Length of the state vector of the system. */
+      43             :     static constexpr int m = n_inputs;            /*!< \brief Length of the input vector of the system. */
+      44             :     static constexpr int p = n_measurements;      /*!< \brief Length of the measurement vector of the system. */
+      45             :     using Base_class = KalmanFilter<n, m, p>; /*!< \brief Base class of this class. */
+      46             : 
+      47             :     using x_t = typename Base_class::x_t;                /*!< \brief State vector type \f$n \times 1\f$ */
+      48             :     using u_t = typename Base_class::u_t;                /*!< \brief Input vector type \f$m \times 1\f$ */
+      49             :     using z_t = typename Base_class::z_t;                /*!< \brief Measurement vector type \f$p \times 1\f$ */
+      50             :     using P_t = typename Base_class::P_t;                /*!< \brief State uncertainty covariance matrix type \f$n \times n\f$ */
+      51             :     using R_t = typename Base_class::R_t;                /*!< \brief Measurement noise covariance matrix type \f$p \times p\f$ */
+      52             :     using Q_t = typename Base_class::Q_t;                /*!< \brief Process noise covariance matrix type \f$n \times n\f$ */
+      53             :     using statecov_t = typename Base_class::statecov_t;  /*!< \brief Helper struct for passing around the state and its covariance in one variable */
+      54             : 
+      55             :     typedef Eigen::Matrix<double, n, n> A_t;  /*!< \brief System transition matrix type \f$n \times n\f$ */
+      56             :     typedef Eigen::Matrix<double, n, m> B_t;  /*!< \brief Input to state mapping matrix type \f$n \times m\f$ */
+      57             :     typedef Eigen::Matrix<double, p, n> H_t;  /*!< \brief State to measurement mapping matrix type \f$p \times n\f$ */
+      58             :     typedef Eigen::Matrix<double, n, p> K_t;  /*!< \brief Kalman gain matrix type \f$n \times p\f$ */
+      59             : 
+      60             :   /*!
+      61             :     * \brief This exception is thrown when taking the inverse of a matrix fails.
+      62             :     *
+      63             :     * You should catch this exception in your code and act accordingly if it appears
+      64             :     * (e.g. reset the state and covariance or modify the measurement/process noise parameters).
+      65             :     */
+      66             :     struct inverse_exception : public std::exception
+      67             :     {
+      68             :     /*!
+      69             :       * \brief Returns the error message, describing what caused the exception.
+      70             :       *
+      71             :       * \return  The error message, describing what caused the exception.
+      72             :       */
+      73           0 :       const char* what() const throw()
+      74             :       {
+      75           0 :         return "LKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)";
+      76             :       }
+      77             :     };
+      78             :     //}
+      79             : 
+      80             :   public:
+      81             :   /*!
+      82             :     * \brief Convenience default constructor.
+      83             :     *
+      84             :     * This constructor should not be used if applicable. If used, the main constructor has to be called afterwards,
+      85             :     * before using this class, otherwise the LKF object is invalid (not initialized).
+      86             :     */
+      87           3 :     LKF(){};
+      88             : 
+      89             :   /*!
+      90             :     * \brief The main constructor.
+      91             :     *
+      92             :     * \param A             The state transition matrix.
+      93             :     * \param B             The input to state mapping matrix.
+      94             :     * \param H             The state to measurement mapping matrix.
+      95             :     */
+      96         271 :     LKF(const A_t& A, const B_t& B, const H_t& H) : A(A), B(B), H(H){};
+      97             : 
+      98             :     /* correct() method //{ */
+      99             :   /*!
+     100             :     * \brief Applies the correction (update, measurement, data) step of the Kalman filter.
+     101             :     *
+     102             :     * This method applies the linear Kalman filter correction step to the state and covariance
+     103             :     * passed in \p sc using the measurement \p z and measurement noise \p R. The updated state
+     104             :     * and covariance after the correction step is returned.
+     105             :     *
+     106             :     * \param sc          The state and covariance to which the correction step is to be applied.
+     107             :     * \param z           The measurement vector to be used for correction.
+     108             :     * \param R           The measurement noise covariance matrix to be used for correction.
+     109             :     * \return            The state and covariance after the correction update.
+     110             :     */
+     111      659190 :     virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const override
+     112             :     {
+     113             :       /* return correct_optimized(sc, z, R, H); */
+     114      659190 :       return correction_impl(sc, z, R, H);
+     115             :     };
+     116             :     //}
+     117             : 
+     118             :     /* predict() method //{ */
+     119             :   /*!
+     120             :     * \brief Applies the prediction (time) step of the Kalman filter.
+     121             :     *
+     122             :     * This method applies the linear Kalman filter prediction step to the state and covariance
+     123             :     * passed in \p sc using the input \p u and process noise \p Q. The process noise covariance
+     124             :     * \p Q is scaled by the \p dt parameter. The updated state and covariance after
+     125             :     * the prediction step is returned.
+     126             :     *
+     127             :     * \param sc          The state and covariance to which the prediction step is to be applied.
+     128             :     * \param u           The input vector to be used for prediction.
+     129             :     * \param Q           The process noise covariance matrix to be used for prediction.
+     130             :     * \param dt          Used to scale the process noise covariance \p Q.
+     131             :     * \return            The state and covariance after the prediction step.
+     132             :     *
+     133             :     * \note Note that the \p dt parameter is only used to scale the process noise covariance \p Q it
+     134             :     * does not change the system matrices #A or #B (because there is no unambiguous way to do this)!
+     135             :     * If you have a changing time step duration and a dynamic system, you have to change the #A and #B
+     136             :     * matrices manually.
+     137             :     */
+     138      469665 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
+     139             :     {
+     140      469665 :       statecov_t ret;
+     141      468449 :       ret.x = state_predict(A, sc.x, B, u);
+     142      468734 :       ret.P = covariance_predict(A, sc.P, Q, dt);
+     143      468155 :       return ret;
+     144             :     };
+     145             :     //}
+     146             : 
+     147             :   public:
+     148             :     A_t A;  /*!< \brief The system transition matrix \f$n \times n\f$ */
+     149             :     B_t B;  /*!< \brief The input to state mapping matrix \f$n \times m\f$ */
+     150             :     H_t H;  /*!< \brief The state to measurement mapping matrix \f$p \times n\f$ */
+     151             : 
+     152             :   protected:
+     153             :     /* covariance_predict() method //{ */
+     154      487004 :     static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
+     155             :     {
+     156      487004 :       return A * P * A.transpose() + dt*Q;
+     157             :     }
+     158             :     //}
+     159             : 
+     160             :     /* state_predict() method //{ */
+     161             :     template <int check = n_inputs>
+     162             :     static inline typename std::enable_if<check == 0, x_t>::type state_predict(const A_t& A, const x_t& x, [[maybe_unused]] const B_t& B,
+     163             :                                                                                [[maybe_unused]] const u_t& u)
+     164             :     {
+     165             :       return A * x;
+     166             :     }
+     167             : 
+     168             :     template <int check = n_inputs>
+     169      486903 :     static inline typename std::enable_if<check != 0, x_t>::type state_predict(const A_t& A, const x_t& x, const B_t& B, const u_t& u)
+     170             :     {
+     171      486903 :       return A * x + B * u;
+     172             :     }
+     173             :     //}
+     174             : 
+     175             :   protected:
+     176             :     /* invert_W() method //{ */
+     177      657720 :     static R_t invert_W(R_t W)
+     178             :     {
+     179      657720 :       Eigen::ColPivHouseholderQR<R_t> qr(W);
+     180      659029 :       if (!qr.isInvertible())
+     181             :       {
+     182             :         // add some stuff to the tmp matrix diagonal to make it invertible
+     183       43532 :         R_t ident = R_t::Identity(W.rows(), W.cols());
+     184       43532 :         W += 1e-9 * ident;
+     185       43545 :         qr.compute(W);
+     186       43569 :         if (!qr.isInvertible())
+     187             :         {
+     188             :           // never managed to make this happen except for explicitly putting NaNs in the input
+     189           0 :           throw inverse_exception();
+     190             :         }
+     191             :       }
+     192      656337 :       const R_t W_inv = qr.inverse();
+     193     1315404 :       return W_inv;
+     194             :     }
+     195             :     //}
+     196             : 
+     197             :     /* computeKalmanGain() method //{ */
+     198      658298 :     virtual K_t computeKalmanGain(const statecov_t& sc, [[maybe_unused]] const z_t& z, const R_t& R, const H_t& H) const
+     199             :     {
+     200             :       // calculation of the kalman gain K
+     201      658298 :       const R_t W = H * sc.P * H.transpose() + R;
+     202      658586 :       const R_t W_inv = invert_W(W);
+     203      656478 :       const K_t K = sc.P * H.transpose() * W_inv;
+     204     1317126 :       return K;
+     205             :     }
+     206             :     //}
+     207             : 
+     208             :     /* correction_impl() method //{ */
+     209             :     template<int check=n>
+     210      659232 :     typename std::enable_if<check >= 0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
+     211             :     {
+     212             :       // the correction phase
+     213      659232 :       statecov_t ret;
+     214      658657 :       const K_t K = computeKalmanGain(sc, z, R, H);
+     215      658928 :       ret.x = sc.x + K * (z - (H * sc.x));
+     216      657986 :       ret.P = (P_t::Identity() - (K * H)) * sc.P;
+     217     1315810 :       return ret;
+     218             :     }
+     219             : 
+     220             :     template<int check=n>
+     221             :     typename std::enable_if<check < 0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
+     222             :     {
+     223             :       // the correction phase
+     224             :       statecov_t ret;
+     225             :       const K_t K = computeKalmanGain(sc, z, R, H);
+     226             :       ret.x = sc.x + K * (z - (H * sc.x));
+     227             :       ret.P = (P_t::Identity(sc.P.rows(), sc.P.cols()) - (K * H)) * sc.P;
+     228             :       return ret;
+     229             :     }
+     230             :     //}
+     231             : 
+     232             :     // NOT USED METHODS
+     233             :     /* correction_optimized() method //{ */
+     234             :     // No notable performance gain was observed for the matrix sizes we use, so this is not used.
+     235           0 :     static statecov_t correction_optimized(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H)
+     236             :     {
+     237           0 :       statecov_t ret = sc;
+     238           0 :       const H_t B(H*sc.P.transpose());
+     239           0 :       const K_t K((B*H.transpose() + R).ldlt().solve(B).transpose());
+     240           0 :       ret.x.noalias() += K*(z - H*sc.x);
+     241           0 :       ret.P.noalias() -= K*H*sc.P;
+     242           0 :       return ret;
+     243             :     }
+     244             : 
+     245             :     /* static statecov_t correction_optimized(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) */
+     246             :     /* { */
+     247             :     /*   statecov_t ret; */
+     248             :     /*   const H_t B = H*sc.P.transpose(); */
+     249             :     /*   const K_t K = (B*H.transpose() + R).partialPivLu().solve(B).transpose(); */
+     250             :     /*   ret.x = sc.x + K*(z - H*sc.x); */
+     251             :     /*   ret.P = sc.P - K*H*sc.P; */
+     252             :     /*   return ret; */
+     253             :     /* } */
+     254             :     //}
+     255             :   };
+     256             :   //}
+     257             : 
+     258             :   /* class dtMatrixLKF //{ */
+     259             :   template <int n_states, int n_inputs, int n_measurements>
+     260             :   class varstepLKF : public LKF<n_states, n_inputs, n_measurements>
+     261             :   {
+     262             :   public:
+     263             :     /* LKF definitions (typedefs, constants etc) //{ */
+     264             :     static const int n = n_states;
+     265             :     static const int m = n_inputs;
+     266             :     static const int p = n_measurements;
+     267             :     using Base_class = LKF<n, m, p>;
+     268             : 
+     269             :     using x_t = typename Base_class::x_t;
+     270             :     using u_t = typename Base_class::u_t;
+     271             :     using z_t = typename Base_class::z_t;
+     272             :     using P_t = typename Base_class::P_t;
+     273             :     using R_t = typename Base_class::R_t;
+     274             :     using statecov_t = typename Base_class::statecov_t;
+     275             :     using A_t = typename Base_class::A_t;  // measurement mapping p*n
+     276             :     using B_t = typename Base_class::B_t;  // process covariance n*n
+     277             :     using H_t = typename Base_class::H_t;  // measurement mapping p*n
+     278             :     using Q_t = typename Base_class::Q_t;  // process covariance n*n
+     279             : 
+     280             :     using generateA_t = std::function<A_t(double)>;
+     281             :     using generateB_t = std::function<B_t(double)>;
+     282             :     //}
+     283             : 
+     284             :   public:
+     285             : 
+     286             :   /*!
+     287             :     * \brief The main constructor.
+     288             :     *
+     289             :     * \param generateA a function, which returns the state transition matrix \p A based on the time difference \p dt.
+     290             :     * \param generateB a function, which returns the input to state mapping matrix \p B based on the time difference \p dt.
+     291             :     * \param H         the state to measurement mapping matrix.
+     292             :     */
+     293           3 :     varstepLKF(const generateA_t& generateA, const generateB_t& generateB, const H_t& H)
+     294           3 :       : m_generateA(generateA), m_generateB(generateB)
+     295             :     {
+     296           3 :       Base_class::H = H;
+     297           3 :     };
+     298             : 
+     299             :     /* predict() method //{ */
+     300             :   /*!
+     301             :     * \brief Applies the prediction (time) step of the Kalman filter.
+     302             :     *
+     303             :     * This method applies the linear Kalman filter prediction step to the state and covariance
+     304             :     * passed in \p sc using the input \p u and process noise \p Q. The process noise covariance
+     305             :     * \p Q is scaled by the \p dt parameter. The updated state and covariance after
+     306             :     * the prediction step is returned.
+     307             :     *
+     308             :     * \param sc          The state and covariance to which the prediction step is to be applied.
+     309             :     * \param u           The input vector to be used for prediction.
+     310             :     * \param Q           The process noise covariance matrix to be used for prediction.
+     311             :     * \param dt          Used to scale the process noise covariance \p Q and to generate the state transition and input to state mapping matrices \p A and \B using the functions, passed in the object's constructor.
+     312             :     * \return            The state and covariance after the prediction step.
+     313             :     *
+     314             :     * \note Note that the \p dt parameter is used to scale the process noise covariance \p Q and to generate the system matrices #A or #B using the functions, passed in the constructor!
+     315             :     */
+     316       17141 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
+     317             :     {
+     318       17141 :       statecov_t ret;
+     319       17141 :       A_t A = m_generateA(dt);
+     320       17141 :       B_t B = m_generateB(dt);
+     321       17141 :       ret.x = Base_class::state_predict(A, sc.x, B, u);
+     322       17141 :       ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
+     323       34282 :       return ret;
+     324             :     };
+     325             :     //}
+     326             :     
+     327             :   private:
+     328             :     generateA_t m_generateA;
+     329             :     generateB_t m_generateB;
+     330             :   };
+     331             :   //}
+     332             : 
+     333             :   /* class LKF_MRS_odom //{ */
+     334             :   class LKF_MRS_odom : public LKF<3, 1, 1>
+     335             :   {
+     336             :   public:
+     337             :     /* LKF definitions (typedefs, constants etc) //{ */
+     338             :     static const int n = 3;
+     339             :     static const int m = 1;
+     340             :     static const int p = 1;
+     341             :     using Base_class = LKF<n, m, p>;
+     342             : 
+     343             :     using x_t = typename Base_class::x_t;
+     344             :     using u_t = typename Base_class::u_t;
+     345             :     using z_t = typename Base_class::z_t;
+     346             :     using P_t = typename Base_class::P_t;
+     347             :     using R_t = typename Base_class::R_t;
+     348             :     using statecov_t = typename Base_class::statecov_t;
+     349             :     using A_t = typename Base_class::A_t;  // measurement mapping p*n
+     350             :     using B_t = typename Base_class::B_t;  // process covariance n*n
+     351             :     using H_t = typename Base_class::H_t;  // measurement mapping p*n
+     352             :     using Q_t = typename Base_class::Q_t;  // process covariance n*n
+     353             : 
+     354             :     using coeff_A_t = A_t;                            // matrix of constant coefficients in matrix A
+     355             :     typedef Eigen::Matrix<unsigned, n, n> dtexp_A_t;  // matrix of dt exponents in matrix A
+     356             :     using coeff_B_t = B_t;                            // matrix of constant coefficients in matrix B
+     357             :     typedef Eigen::Matrix<unsigned, n, m> dtexp_B_t;  // matrix of dt exponents in matrix B
+     358             :     //}
+     359             : 
+     360             :   public:
+     361             :     LKF_MRS_odom(const std::vector<H_t>& Hs, const double default_dt = 1);
+     362             :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override;
+     363             :     virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R, int param = 0) const;
+     364             : 
+     365             :   public:
+     366             :     x_t state_predict_optimized(const x_t& x_prev, const u_t& u, double dt) const;
+     367             :     P_t covariance_predict_optimized(const P_t& P, const Q_t& Q, double dt) const;
+     368             : 
+     369             :   private:
+     370             :     std::vector<H_t> m_Hs;
+     371             :   };
+     372             :   //}
+     373             : 
+     374             : }  // namespace mrs_lib
+     375             : 
+     376             : #endif // LKFSYSTEMMODELS_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/lkf.h.gcov.overview.html b/mrs_lib/include/mrs_lib/lkf.h.gcov.overview.html new file mode 100644 index 0000000000..1e9597d534 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.gcov.overview.html @@ -0,0 +1,114 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/lkf.h.gcov.png b/mrs_lib/include/mrs_lib/lkf.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..05cf672e576224906233fe48ec21708336bc253d GIT binary patch literal 1750 zcmV;{1}XW8P)=6legnx+l_P+MYm!lPS#C}Nxki0KGD^nk)CSbq7WG>_kok@cd06$*#L+i(P!undeF?0}4}V4Hzz zJ18#dU<`x^)|f2w8ts?S9mr(jg5_EX9Z{SlgrGC5OBg5+90x}FcnhS}ATGu}PHqq0 z9H`2W#l_^TZjekUtj_|*&rwA46m~SJJrxlejK}O~=|G+dGlAqyJ)i_=7cgR#(qj#A z&?)?>Um0-Rk%*Z-AQ7e`$lN9OwM zTGJTxEY2F*Hs#m~I6zSda%m~6O3Ec=ZNh8dDk12SpWFt&TyD#_bjQ?(5OCOzf!Fg6 zaEKz_$_m4n*8>6ozou&4L(U}oE&9`_ra*gT@5P^rtTT^ za5S4Hav>kMm71um62Iq!CF?`;j5Hv@(%H=RnARB`3aTZC$vTCR3#iDm%|#WcT@>|@ z1cYhJIvOoA%@H?C&a}AJ&0MbOdR_iPmjyU`-E87XqX9T;Tht6V`XiD)up9L zlUN&uisw$!vkHf`hDf4>qHY$dB4f}JJ@E1h%s+5q$LZu|3K$ELk*po}=Z?n5LJCHT z{FDPY&k|8`P^mn#22x7h(w89Blk}rOYLm&@NRY1s`fNQ@q*{$g%KSclB zEhEPszc#6*cbwCle+1z)4_7m@34Gbru0m4{Y23qOa3$`68(+$rHZPJG-HdF#6S21_XQZI(&$ z;z*Fzhrm{Y{EZ8B~yZ~tVV^X0VsN{DI9_ks3Bh1L@r;A&0c5MZFi z9b+?hSzsH*z`iOY=JD>3_+alU0C@XkS1XFk;woBKuY!#Dm~O~}iKYj(njK+Gwb8vG zVcUEgXZ4CrgZCj|8(Z3E?7f>d#rg{@zZA&ku?IREe1&3h{s}!Ghr%~fbPtKbs|w^V zfS=p&byNKGVZ8X6ZZ((ZG#wzYXUIWLBE5eUKX@3g?Ud~3t$xiLLz~{Z(!?Ivyqb6R z!Y(5R5H8f^=Uy22fB3mF&$k|a23>_ebNE>bG5wYkNVA)960qDBzEw{)>%hv%=C3)^ zbk4Q}_udm;;8(N&y@;zjBC?jNz%QxnyE{#%1OHd{9Uz@jm~LC&HiEihOx@MQ$}24z-zRls#8bC&;+v)$A(vKiojj3b#V sYQfaRxmlm>Jiz7HX$@zTer%uSA5hR)7d&;<(*OVf07*qoM6N<$f>Mk-2LJ#7 literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html b/mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html new file mode 100644 index 0000000000..ab56d19948 --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-04-25 05:42:57Functions:154434.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::getHeading(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::Reference_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::TwistWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::Twist_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::Reference_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(geometry_msgs::TwistWithCovariance_<std::allocator<void> > const&)0
mrs_lib::getVelocity(geometry_msgs::Twist_<std::allocator<void> > const&)0
mrs_lib::getVelocity(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getVelocity(nav_msgs::Odometry_<std::allocator<void> > const&)0
mrs_lib::getXYZ(geometry_msgs::Vector3_<std::allocator<void> > const&)0
mrs_lib::getXYZ(boost::shared_ptr<geometry_msgs::Point_<std::allocator<void> > const> const&)0
mrs_lib::getXYZ(boost::shared_ptr<geometry_msgs::Vector3_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)0
mrs_lib::getYaw(geometry_msgs::Pose_<std::allocator<void> > const&)0
mrs_lib::getYaw(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(nav_msgs::Odometry_<std::allocator<void> > const&)0
mrs_lib::getPose(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)360
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)360
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)360
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)360
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)360
mrs_lib::getPose(nav_msgs::Odometry_<std::allocator<void> > const&)1682
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)220779
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)220779
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)220781
mrs_lib::getHeading(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)220781
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)220782
mrs_lib::getPosition(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)220782
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)220782
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)220783
mrs_lib::getXYZ(geometry_msgs::Point_<std::allocator<void> > const&)221503
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/msg_extractor.h.func.html b/mrs_lib/include/mrs_lib/msg_extractor.h.func.html new file mode 100644 index 0000000000..ed3f1456fd --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.func.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-04-25 05:42:57Functions:154434.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)220781
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)220779
mrs_lib::getHeading(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::Reference_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)220781
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)360
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)360
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)220779
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)220783
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)220782
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::TwistWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::Twist_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::Reference_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)220782
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)360
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)360
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)360
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)220782
mrs_lib::getVelocity(geometry_msgs::TwistWithCovariance_<std::allocator<void> > const&)0
mrs_lib::getVelocity(geometry_msgs::Twist_<std::allocator<void> > const&)0
mrs_lib::getVelocity(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getVelocity(nav_msgs::Odometry_<std::allocator<void> > const&)0
mrs_lib::getXYZ(geometry_msgs::Point_<std::allocator<void> > const&)221503
mrs_lib::getXYZ(geometry_msgs::Vector3_<std::allocator<void> > const&)0
mrs_lib::getXYZ(boost::shared_ptr<geometry_msgs::Point_<std::allocator<void> > const> const&)0
mrs_lib::getXYZ(boost::shared_ptr<geometry_msgs::Vector3_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)0
mrs_lib::getYaw(geometry_msgs::Pose_<std::allocator<void> > const&)0
mrs_lib::getYaw(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(nav_msgs::Odometry_<std::allocator<void> > const&)0
mrs_lib::getPose(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getPose(nav_msgs::Odometry_<std::allocator<void> > const&)1682
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.frameset.html new file mode 100644 index 0000000000..3c229c2d36 --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html new file mode 100644 index 0000000000..9b804c32a1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html @@ -0,0 +1,775 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-04-25 05:42:57Functions:154434.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: TomasFormat
+       2             : /**  \file
+       3             :  *   \brief utility functions for getting stuff from ROS msgs
+       4             :  *   \author Tomas Baca - tomas.baca@fel.cvut.cz
+       5             :  */
+       6             : #ifndef MRS_LIB_MSG_EXTRACTOR_H
+       7             : #define MRS_LIB_MSG_EXTRACTOR_H
+       8             : 
+       9             : #include <mrs_msgs/TrackerCommand.h>
+      10             : #include <mrs_msgs/Reference.h>
+      11             : #include <mrs_msgs/ReferenceStamped.h>
+      12             : 
+      13             : #include <nav_msgs/Odometry.h>
+      14             : 
+      15             : #include <mrs_lib/attitude_converter.h>
+      16             : 
+      17             : namespace mrs_lib
+      18             : {
+      19             : 
+      20             : /* geometry_msgs::Point //{ */
+      21             : 
+      22             : /**
+      23             :  * @brief get XYZ from geometry_msgs::Point
+      24             :  *
+      25             :  * @param data point
+      26             :  *
+      27             :  * @return x, y, z
+      28             :  */
+      29      221503 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Point& data) {
+      30             : 
+      31      221503 :   double x = data.x;
+      32      221503 :   double y = data.y;
+      33      221503 :   double z = data.z;
+      34             : 
+      35      443003 :   return std::tuple(x, y, z);
+      36             : }
+      37             : 
+      38             : /**
+      39             :  * @brief get XYZ from geometry_msgs::PointConstPtr
+      40             :  *
+      41             :  * @param data point (ConstPtr)
+      42             :  *
+      43             :  * @return x, y, z
+      44             :  */
+      45           0 : std::tuple<double, double, double> getXYZ(const geometry_msgs::PointConstPtr& data) {
+      46             : 
+      47           0 :   return getXYZ(*data);
+      48             : }
+      49             : 
+      50             : //}
+      51             : 
+      52             : /* geometry_msgs::Vector3 //{ */
+      53             : 
+      54             : /**
+      55             :  * @brief get XYZ from geometry_msgs::Vector3
+      56             :  *
+      57             :  * @param data vector3
+      58             :  *
+      59             :  * @return x, y, z
+      60             :  */
+      61           0 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Vector3& data) {
+      62             : 
+      63           0 :   double x = data.x;
+      64           0 :   double y = data.y;
+      65           0 :   double z = data.z;
+      66             : 
+      67           0 :   return std::tuple(x, y, z);
+      68             : }
+      69             : 
+      70             : /**
+      71             :  * @brief get XYZ from geometry_msgs::Vector3ConstPtr
+      72             :  *
+      73             :  * @param data vector3 (ConstPtr)
+      74             :  *
+      75             :  * @return x, y, z
+      76             :  */
+      77           0 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Vector3ConstPtr& data) {
+      78             : 
+      79           0 :   return getXYZ(*data);
+      80             : }
+      81             : 
+      82             : //}
+      83             : 
+      84             : /* geometry_msgs::Pose //{ */
+      85             : 
+      86             : /* getPosition() //{ */
+      87             : 
+      88             : /**
+      89             :  * @brief get position from geometry_msgs::Pose
+      90             :  *
+      91             :  * @param data pose
+      92             :  *
+      93             :  * @return x, y, z
+      94             :  */
+      95      220782 : std::tuple<double, double, double> getPosition(const geometry_msgs::Pose& data) {
+      96             : 
+      97      220782 :   return getXYZ(data.position);
+      98             : }
+      99             : 
+     100             : /**
+     101             :  * @brief get position from geometry_msgs::PoseConstPtr
+     102             :  *
+     103             :  * @param data pose (ConstPtr)
+     104             :  *
+     105             :  * @return x, y, z
+     106             :  */
+     107           0 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseConstPtr& data) {
+     108             : 
+     109           0 :   return getPosition(*data);
+     110             : }
+     111             : 
+     112             : //}
+     113             : 
+     114             : /* getHeading() //{ */
+     115             : 
+     116             : /**
+     117             :  * @brief get heading from geometry_msgs::Pose
+     118             :  *
+     119             :  * @param data pose
+     120             :  *
+     121             :  * @return heading
+     122             :  */
+     123      220779 : double getHeading(const geometry_msgs::Pose& data) {
+     124             : 
+     125      220779 :   return mrs_lib::AttitudeConverter(data.orientation).getHeading();
+     126             : }
+     127             : 
+     128             : /**
+     129             :  * @brief get heading from geometry_msgs::PoseConstPtr
+     130             :  *
+     131             :  * @param data pose (ConstPtr)
+     132             :  *
+     133             :  * @return heading
+     134             :  */
+     135           0 : double getHeading(const geometry_msgs::PoseConstPtr& data) {
+     136             : 
+     137           0 :   return getHeading(*data);
+     138             : }
+     139             : 
+     140             : //}
+     141             : 
+     142             : /* getYaw() //{ */
+     143             : 
+     144             : /**
+     145             :  * @brief get yaw from geometry_msgs::Pose
+     146             :  *
+     147             :  * @param data pose
+     148             :  *
+     149             :  * @return yaw
+     150             :  */
+     151           0 : double getYaw(const geometry_msgs::Pose& data) {
+     152             : 
+     153           0 :   return mrs_lib::AttitudeConverter(data.orientation).getYaw();
+     154             : }
+     155             : 
+     156             : /**
+     157             :  * @brief get yaw from geometry_msgs::PoseConstPtr
+     158             :  *
+     159             :  * @param data pose (ConstPtr)
+     160             :  *
+     161             :  * @return yaw
+     162             :  */
+     163           0 : double getYaw(const geometry_msgs::PoseConstPtr& data) {
+     164             : 
+     165           0 :   return getYaw(*data);
+     166             : }
+     167             : 
+     168             : //}
+     169             : 
+     170             : //}
+     171             : 
+     172             : /* geometry_msgs::PoseWithCovariance //{ */
+     173             : 
+     174             : /* getPosition() //{ */
+     175             : 
+     176             : /**
+     177             :  * @brief get position from geometry_msgs::PoseWithCovariance
+     178             :  *
+     179             :  * @param data pose with covariance
+     180             :  *
+     181             :  * @return x, y, z
+     182             :  */
+     183      220783 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovariance& data) {
+     184             : 
+     185      220783 :   return getPosition(data.pose);
+     186             : }
+     187             : 
+     188             : /**
+     189             :  * @brief get position from geometry_msgs::PoseWithCovarianceConstPtr
+     190             :  *
+     191             :  * @param data pose with covariance (ConstPtr)
+     192             :  *
+     193             :  * @return x, y, z
+     194             :  */
+     195           0 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovarianceConstPtr& data) {
+     196             : 
+     197           0 :   return getPosition(*data);
+     198             : }
+     199             : 
+     200             : //}
+     201             : 
+     202             : /* getHeading() //{ */
+     203             : 
+     204             : /**
+     205             :  * @brief get heading from geometry_msgs::PoseWithCovariance
+     206             :  *
+     207             :  * @param data pose with covariance
+     208             :  *
+     209             :  * @return heading
+     210             :  */
+     211      220781 : double getHeading(const geometry_msgs::PoseWithCovariance& data) {
+     212             : 
+     213      220781 :   return getHeading(data.pose);
+     214             : }
+     215             : 
+     216             : /**
+     217             :  * @brief get heading from geometry_msgs::PoseWithCovarianceConstPtr
+     218             :  *
+     219             :  * @param data pose with covariance (ConstPtr)
+     220             :  *
+     221             :  * @return heading
+     222             :  */
+     223           0 : double getHeading(const geometry_msgs::PoseWithCovarianceConstPtr& data) {
+     224             : 
+     225           0 :   return getHeading(*data);
+     226             : }
+     227             : 
+     228             : //}
+     229             : 
+     230             : /* getYaw() //{ */
+     231             : 
+     232             : /**
+     233             :  * @brief get yaw from geometry_msgs::PoseWithCovariance
+     234             :  *
+     235             :  * @param data pose with covariance
+     236             :  *
+     237             :  * @return yaw
+     238             :  */
+     239           0 : double getYaw(const geometry_msgs::PoseWithCovariance& data) {
+     240             : 
+     241           0 :   return getYaw(data.pose);
+     242             : }
+     243             : 
+     244             : /**
+     245             :  * @brief get yaw from geometry_msgs::PoseWithCovarianceConstPtr
+     246             :  *
+     247             :  * @param data pose with covariance (ConstPtr)
+     248             :  *
+     249             :  * @return yaw
+     250             :  */
+     251           0 : double getYaw(const geometry_msgs::PoseWithCovarianceConstPtr& data) {
+     252             : 
+     253           0 :   return getYaw(*data);
+     254             : }
+     255             : 
+     256             : //}
+     257             : 
+     258             : //}
+     259             : 
+     260             : /* geometry_msgs::Twist //{ */
+     261             : 
+     262             : /* getVelocity() //{ */
+     263             : 
+     264             : /**
+     265             :  * @brief get velocity from geometry_msgs::Twist
+     266             :  *
+     267             :  * @param data twist
+     268             :  *
+     269             :  * @return x, y, z
+     270             :  */
+     271           0 : std::tuple<double, double, double> getVelocity(const geometry_msgs::Twist& data) {
+     272             : 
+     273           0 :   return getXYZ(data.linear);
+     274             : }
+     275             : 
+     276             : /**
+     277             :  * @brief get position from geometry_msgs::TwistConstPtr
+     278             :  *
+     279             :  * @param data twist (ConstPtr)
+     280             :  *
+     281             :  * @return x, y, z
+     282             :  */
+     283           0 : std::tuple<double, double, double> getPosition(const geometry_msgs::TwistConstPtr& data) {
+     284             : 
+     285           0 :   return getVelocity(*data);
+     286             : }
+     287             : 
+     288             : //}
+     289             : 
+     290             : //}
+     291             : 
+     292             : /* geometry_msgs::TwistWithCovariance //{ */
+     293             : 
+     294             : /* getVelocity() //{ */
+     295             : 
+     296             : /**
+     297             :  * @brief get velocity from geometry_msgs::TwistWithCovariance
+     298             :  *
+     299             :  * @param data twistwithcovariance
+     300             :  *
+     301             :  * @return x, y, z
+     302             :  */
+     303           0 : std::tuple<double, double, double> getVelocity(const geometry_msgs::TwistWithCovariance& data) {
+     304             : 
+     305           0 :   return getVelocity(data.twist);
+     306             : }
+     307             : 
+     308             : /**
+     309             :  * @brief get position from geometry_msgs::TwistWithCovarianceConstPtr
+     310             :  *
+     311             :  * @param data twistwithcovariance (ConstPtr)
+     312             :  *
+     313             :  * @return x, y, z
+     314             :  */
+     315           0 : std::tuple<double, double, double> getPosition(const geometry_msgs::TwistWithCovarianceConstPtr& data) {
+     316             : 
+     317           0 :   return getVelocity(*data);
+     318             : }
+     319             : 
+     320             : //}
+     321             : 
+     322             : //}
+     323             : 
+     324             : /* nav_msgs::Odometry //{ */
+     325             : 
+     326             : /* getPosition() //{ */
+     327             : 
+     328             : /**
+     329             :  * @brief get position from nav_msgs::Odometry
+     330             :  *
+     331             :  * @param data odometry
+     332             :  *
+     333             :  * @return x, y, z
+     334             :  */
+     335      220782 : std::tuple<double, double, double> getPosition(const nav_msgs::Odometry& data) {
+     336             : 
+     337      220782 :   return getPosition(data.pose);
+     338             : }
+     339             : 
+     340             : /**
+     341             :  * @brief get position from nav_msgs::OdometryConstPtr
+     342             :  *
+     343             :  * @param data odometry (ConstPtr)
+     344             :  *
+     345             :  * @return x, y, z
+     346             :  */
+     347      220782 : std::tuple<double, double, double> getPosition(const nav_msgs::OdometryConstPtr& data) {
+     348             : 
+     349      220782 :   return getPosition(*data);
+     350             : }
+     351             : 
+     352             : //}
+     353             : 
+     354             : /* getVelocity() //{ */
+     355             : 
+     356             : /**
+     357             :  * @brief get position from nav_msgs::Odometry
+     358             :  *
+     359             :  * @param data odometry
+     360             :  *
+     361             :  * @return x, y, z
+     362             :  */
+     363           0 : std::tuple<double, double, double> getVelocity(const nav_msgs::Odometry& data) {
+     364             : 
+     365           0 :   return getVelocity(data.twist);
+     366             : }
+     367             : 
+     368             : /**
+     369             :  * @brief get velocity from nav_msgs::OdometryConstPtr
+     370             :  *
+     371             :  * @param data odometry (ConstPtr)
+     372             :  *
+     373             :  * @return x, y, z
+     374             :  */
+     375           0 : std::tuple<double, double, double> getVelocity(const nav_msgs::OdometryConstPtr& data) {
+     376             : 
+     377           0 :   return getVelocity(*data);
+     378             : }
+     379             : 
+     380             : //}
+     381             : 
+     382             : /* getHeading() //{ */
+     383             : 
+     384             : /**
+     385             :  * @brief get heading from nav_msgs::Odometry
+     386             :  *
+     387             :  * @param data odometry
+     388             :  *
+     389             :  * @return heading
+     390             :  */
+     391      220779 : double getHeading(const nav_msgs::Odometry& data) {
+     392             : 
+     393      220779 :   return getHeading(data.pose);
+     394             : }
+     395             : 
+     396             : /**
+     397             :  * @brief get heading from nav_msgs::OdometryConstPtr
+     398             :  *
+     399             :  * @param data odometry (ConstPtr)
+     400             :  *
+     401             :  * @return heading
+     402             :  */
+     403      220781 : double getHeading(const nav_msgs::OdometryConstPtr& data) {
+     404             : 
+     405      220781 :   return getHeading(*data);
+     406             : }
+     407             : 
+     408             : //}
+     409             : 
+     410             : /* getYaw() //{ */
+     411             : 
+     412             : /**
+     413             :  * @brief get yaw from nav_msgs::Odometry
+     414             :  *
+     415             :  * @param data odometry
+     416             :  *
+     417             :  * @return yaw
+     418             :  */
+     419           0 : double getYaw(const nav_msgs::Odometry& data) {
+     420             : 
+     421           0 :   return getYaw(data.pose);
+     422             : }
+     423             : 
+     424             : /**
+     425             :  * @brief get yaw from nav_msgs::OdometryConstPtr
+     426             :  *
+     427             :  * @param data odometry (ConstPtr)
+     428             :  *
+     429             :  * @return yaw
+     430             :  */
+     431           0 : double getYaw(const nav_msgs::OdometryConstPtr& data) {
+     432             : 
+     433           0 :   return getYaw(*data);
+     434             : }
+     435             : 
+     436             : //}
+     437             : 
+     438             : /* getPose() //{ */
+     439             : 
+     440             : /**
+     441             :  * @brief returns the Pose part of the nav_msgs::Odometry message
+     442             :  *
+     443             :  * @param data odometry
+     444             :  *
+     445             :  * @return pose
+     446             :  */
+     447        1682 : geometry_msgs::Pose getPose(const nav_msgs::Odometry& data) {
+     448             : 
+     449        1682 :   return data.pose.pose;
+     450             : }
+     451             : 
+     452             : /**
+     453             :  * @brief returns the Pose part of the nav_msgs::OdometryConstPtr message
+     454             :  *
+     455             :  * @param data odometry (ConstPtr)
+     456             :  *
+     457             :  * @return pose
+     458             :  */
+     459           0 : geometry_msgs::Pose getPose(const nav_msgs::OdometryConstPtr& data) {
+     460             : 
+     461           0 :   return getPose(*data);
+     462             : }
+     463             : 
+     464             : //}
+     465             : 
+     466             : //}
+     467             : 
+     468             : /* mrs_msgs::TrackerCommand //{ */
+     469             : 
+     470             : /* getPosition() //{ */
+     471             : 
+     472             : /**
+     473             :  * @brief get position data from mrs_msgs::TrackerCommand
+     474             :  *
+     475             :  * @param data position command
+     476             :  *
+     477             :  * @return x, y, z
+     478             :  */
+     479         360 : std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommand& data) {
+     480             : 
+     481         360 :   return getXYZ(data.position);
+     482             : }
+     483             : 
+     484             : /**
+     485             :  * @brief get position data from mrs_msgs::TrackerCommandConstPtr
+     486             :  *
+     487             :  * @param data position command (ConstPtr)
+     488             :  *
+     489             :  * @return x, y, z
+     490             :  */
+     491           0 : std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommandConstPtr& data) {
+     492             : 
+     493           0 :   return getPosition(*data);
+     494             : }
+     495             : 
+     496             : //}
+     497             : 
+     498             : /* getVelocity() //{ */
+     499             : 
+     500             : /**
+     501             :  * @brief get velocity data from mrs_msgs::TrackerCommand
+     502             :  *
+     503             :  * @param data position command
+     504             :  *
+     505             :  * @return x, y, z
+     506             :  */
+     507           0 : std::tuple<double, double, double> getVelocity(const mrs_msgs::TrackerCommand& data) {
+     508             : 
+     509           0 :   return getXYZ(data.velocity);
+     510             : }
+     511             : 
+     512             : /**
+     513             :  * @brief get velocity data from mrs_msgs::TrackerCommandConstPtr
+     514             :  *
+     515             :  * @param data position command (ConstPtr)
+     516             :  *
+     517             :  * @return x, y, z
+     518             :  */
+     519           0 : std::tuple<double, double, double> getVelocity(const mrs_msgs::TrackerCommandConstPtr& data) {
+     520             : 
+     521           0 :   return getVelocity(*data);
+     522             : }
+     523             : 
+     524             : //}
+     525             : 
+     526             : /* getHeading() //{ */
+     527             : 
+     528             : /**
+     529             :  * @brief get heading from mrs_msgs::TrackerCommand
+     530             :  *
+     531             :  * @param data position command
+     532             :  *
+     533             :  * @return heading
+     534             :  */
+     535           0 : double getHeading(const mrs_msgs::TrackerCommand& data) {
+     536             : 
+     537           0 :   double heading = 0;
+     538             : 
+     539           0 :   if (data.use_heading) {
+     540             : 
+     541           0 :     heading = data.heading;
+     542             : 
+     543           0 :   } else if (data.use_orientation) {
+     544             : 
+     545           0 :     heading = mrs_lib::AttitudeConverter(data.orientation).getHeading();
+     546             :   }
+     547             : 
+     548           0 :   return heading;
+     549             : }
+     550             : 
+     551             : /**
+     552             :  * @brief get heading from mrs_msgs::TrackerCommandConstPtr
+     553             :  *
+     554             :  * @param data position command (ConstPtr)
+     555             :  *
+     556             :  * @return heading
+     557             :  */
+     558           0 : double getHeading(const mrs_msgs::TrackerCommandConstPtr& data) {
+     559             : 
+     560           0 :   return getHeading(*data);
+     561             : }
+     562             : 
+     563             : //}
+     564             : 
+     565             : //}
+     566             : 
+     567             : /* mrs_msgs::Reference //{ */
+     568             : 
+     569             : /* getPosition() //{ */
+     570             : 
+     571             : /**
+     572             :  * @brief get position from mrs_msgs::Reference
+     573             :  *
+     574             :  * @param data reference
+     575             :  *
+     576             :  * @return x, y, z
+     577             :  */
+     578         360 : std::tuple<double, double, double> getPosition(const mrs_msgs::Reference& data) {
+     579             : 
+     580         360 :   return getXYZ(data.position);
+     581             : }
+     582             : 
+     583             : /**
+     584             :  * @brief get position from mrs_msgs::ReferenceConstPtr
+     585             :  *
+     586             :  * @param data reference (ContrPtr)
+     587             :  *
+     588             :  * @return x, y, z
+     589             :  */
+     590           0 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceConstPtr& data) {
+     591             : 
+     592           0 :   return getPosition(*data);
+     593             : }
+     594             : 
+     595             : //}
+     596             : 
+     597             : /* getHeading() //{ */
+     598             : 
+     599             : /**
+     600             :  * @brief get heading from mrs_msgs::Reference
+     601             :  *
+     602             :  * @param data reference
+     603             :  *
+     604             :  * @return heading
+     605             :  */
+     606         360 : double getHeading(const mrs_msgs::Reference& data) {
+     607             : 
+     608         360 :   return data.heading;
+     609             : }
+     610             : 
+     611             : /**
+     612             :  * @brief get heading from mrs_msgs::ReferenceConstPtr
+     613             :  *
+     614             :  * @param data reference (ContrPtr)
+     615             :  *
+     616             :  * @return heading
+     617             :  */
+     618           0 : double getHeading(const mrs_msgs::ReferenceConstPtr& data) {
+     619             : 
+     620           0 :   return getHeading(*data);
+     621             : }
+     622             : 
+     623             : //}
+     624             : 
+     625             : //}
+     626             : 
+     627             : /* mrs_msgs::ReferenceStamped //{ */
+     628             : 
+     629             : /* getPosition() //{ */
+     630             : 
+     631             : /**
+     632             :  * @brief get position from mrs_msgs::ReferenceStamped
+     633             :  *
+     634             :  * @param data reference stamped
+     635             :  *
+     636             :  * @return x, y, z
+     637             :  */
+     638         360 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStamped& data) {
+     639             : 
+     640         360 :   return getPosition(data.reference);
+     641             : }
+     642             : 
+     643             : /**
+     644             :  * @brief get position from mrs_msgs::ReferenceStampedConstPtr
+     645             :  *
+     646             :  * @param data reference stamped (ContrPtr)
+     647             :  *
+     648             :  * @return x, y, z
+     649             :  */
+     650           0 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStampedConstPtr& data) {
+     651             : 
+     652           0 :   return getPosition(*data);
+     653             : }
+     654             : 
+     655             : //}
+     656             : 
+     657             : /* getHeading() //{ */
+     658             : 
+     659             : /**
+     660             :  * @brief get heading from mrs_msgs::ReferenceStamped
+     661             :  *
+     662             :  * @param data referencestamped
+     663             :  *
+     664             :  * @return heading
+     665             :  */
+     666         360 : double getHeading(const mrs_msgs::ReferenceStamped& data) {
+     667             : 
+     668         360 :   return getHeading(data.reference);
+     669             : }
+     670             : 
+     671             : /**
+     672             :  * @brief get heading from mrs_msgs::ReferenceStampedConstPtr
+     673             :  *
+     674             :  * @param data referencestamped (ContrPtr)
+     675             :  *
+     676             :  * @return heading
+     677             :  */
+     678           0 : double getHeading(const mrs_msgs::ReferenceStampedConstPtr& data) {
+     679             : 
+     680           0 :   return getHeading(*data);
+     681             : }
+     682             : 
+     683             : //}
+     684             : 
+     685             : //}
+     686             : 
+     687             : }  // namespace mrs_lib
+     688             : 
+     689             : //}
+     690             : 
+     691             : #endif  // MRS_LIB_MSG_EXTRACTOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html new file mode 100644 index 0000000000..41496f11e9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html @@ -0,0 +1,193 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.png b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8043ecf807c268d9ebf3ec73e92b93e3796c8069 GIT binary patch literal 1287 zcmV+i1^D`jP)o6G8|QE17xcIDPQ~O}I%wnMSorb!w5W z0Y!DQCakha+q@xH*lxd^|F-j=&QM(1pFiEYu!)Tr=-RiB`&d+9L}*2{sBI|FFr12D z9}Ps**N^XGBATa9-HYGtB}|XjE7fZGp0&-MU3~y zLwt28Vw=vzJ;CfTj(Z+oiqUxIVNwc_du-_?agUWt8H(7Zb8!!+tu>-BvZwjuL8lZF z_W=BI9qwUdp480VsfcYl8}}d-qy2I8d18`3+Vb&2kyV_fh;2F>_aGEgZ0)l5U8}gP z>t;OloV>cwE*D6123-0OUKcJx&&|AwB62gAtu<36*qS4=uXUkU?ob!nLAyBj|LCDIBo5(3y(=!tqZvj(|3Ff^Oxrygkox~V3rHN<&Ti95B4t4JqSex zTOaIQ>q0I>{^!l))rGbqw&^Q8!G@|8270X^`{@VM)(o-mdQXQL}`1POm8-^ zau49yAH}3S&Fy7RBgD~S;nnsu8QvOV<(}-ecG#O}Yrvk@$$RANX*%yM&pimmgnh2{ zElNAYkz(P+_B5^g%X1GxF>z1RviDv0G+W+^E7~8`?vdSkILz*ox?h&(p74<3*8qAr z`Ju%^P_cO49&vWC!xZr7{R2%1_Tc+-T~z=8002ovPDHLkV1gERfj9sF literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html b/mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html new file mode 100644 index 0000000000..88fc3a56bb --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html @@ -0,0 +1,400 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-04-25 05:42:57Functions:678083.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::tuple<double, double, double, double> mrs_lib::get_mutexed<double, double, double, double>(std::mutex&, double&, double&, double&, double&)0
mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const>(std::mutex&, mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)0
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&)0
Eigen::Matrix<double, 2, 2, 0, 2, 2> mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::SpeedTrackerCommand_<std::allocator<void> >&)0
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 1, 0, 2, 1> >(std::mutex&, Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(std::mutex&, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(std::mutex&, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 1, 0, 6, 1> >(std::mutex&, Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Matrix<double, 6, 1, 0, 6, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 6, 0, 6, 6> >(std::mutex&, Eigen::Matrix<double, 6, 6, 0, 6, 6>, Eigen::Matrix<double, 6, 6, 0, 6, 6>&)0
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&)1
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)39
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)94
auto mrs_lib::set_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)134
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)188
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)189
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)437
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)731
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)1107
auto mrs_lib::set_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)2138
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controller::Gains_t>(std::mutex&, mrs_uav_controllers::se3_controller::Gains_t, mrs_uav_controllers::se3_controller::Gains_t&)4539
int mrs_lib::get_mutexed<int>(std::mutex&, int&)8213
std::tuple<bool, bool, ros::Time, ros::Time> mrs_lib::get_mutexed<bool, bool, ros::Time, ros::Time>(std::mutex&, bool&, bool&, ros::Time&, ros::Time&)12811
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controller::Gains_t>(std::mutex&, mrs_uav_controllers::mpc_controller::Gains_t, mrs_uav_controllers::mpc_controller::Gains_t&)14349
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)15372
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)17066
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)17162
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const mrs_lib::get_mutexed<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const>(std::mutex&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)17984
std::tuple<double, double, double, double, double, double, double> mrs_lib::get_mutexed<double, double, double, double, double, double, double>(std::mutex&, double&, double&, double&, double&, double&, double&, double&)23295
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)27925
mrs_uav_controllers::se3_controller::Gains_t mrs_lib::get_mutexed<mrs_uav_controllers::se3_controller::Gains_t>(std::mutex&, mrs_uav_controllers::se3_controller::Gains_t&)32830
auto mrs_lib::set_mutexed<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::mutex&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)34471
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)58400
std::tuple<Eigen::Matrix<double, -1, 1, 0, -1, 1>, double> mrs_lib::get_mutexed<Eigen::Matrix<double, -1, 1, 0, -1, 1>, double>(std::mutex&, Eigen::Matrix<double, -1, 1, 0, -1, 1>&, double&)70897
mrs_msgs::MpcPredictionFullState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::MpcPredictionFullState_<std::allocator<void> > >(std::mutex&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> >&)70897
mrs_uav_controllers::mpc_controller::Gains_t mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controller::Gains_t>(std::mutex&, mrs_uav_controllers::mpc_controller::Gains_t&)75201
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)77111
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)77111
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::get_mutexed<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::mutex&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)81639
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)100473
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)136863
auto mrs_lib::set_mutexed<mrs_msgs::Float64Stamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::Float64Stamped_<std::allocator<void> >, mrs_msgs::Float64Stamped_<std::allocator<void> >&)139277
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)152028
Eigen::Matrix<double, 6, 6, 0, 6, 6> mrs_lib::get_mutexed<Eigen::Matrix<double, 6, 6, 0, 6, 6> >(std::mutex&, Eigen::Matrix<double, 6, 6, 0, 6, 6>&)172034
Eigen::Matrix<double, 1, 1, 0, 1, 1> const mrs_lib::get_mutexed<Eigen::Matrix<double, 1, 1, 0, 1, 1> const>(std::mutex&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)183558
mrs_msgs::UavState_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > const>(std::mutex&, mrs_msgs::UavState_<std::allocator<void> > const&)184615
Eigen::Matrix<double, 2, 2, 0, 2, 2> const mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> const>(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&)190532
auto mrs_lib::set_mutexed<std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >(std::mutex&, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&)206773
std::tuple<std::optional<double>, std::optional<double> > mrs_lib::get_mutexed<std::optional<double>, std::optional<double> >(std::mutex&, std::optional<double>&, std::optional<double>&)207796
std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > mrs_lib::get_mutexed<std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >(std::mutex&, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&)224958
std::tuple<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > mrs_lib::get_mutexed<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::mutex&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&)228488
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::get_mutexed<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::mutex&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&)246211
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)263403
mrs_msgs::Float64Stamped_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::Float64Stamped_<std::allocator<void> > const>(std::mutex&, mrs_msgs::Float64Stamped_<std::allocator<void> > const&)280826
Eigen::Matrix<double, 3, 3, 0, 3, 3> mrs_lib::get_mutexed<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(std::mutex&, Eigen::Matrix<double, 3, 3, 0, 3, 3>&)296113
std::tuple<mrs_msgs::UavState_<std::allocator<void> >, double> mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> >, double>(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&, double&)313813
std::vector<double, std::allocator<double> > const mrs_lib::get_mutexed<std::vector<double, std::allocator<double> > const>(std::mutex&, std::vector<double, std::allocator<double> > const&)317260
nav_msgs::Odometry_<std::allocator<void> > const mrs_lib::get_mutexed<nav_msgs::Odometry_<std::allocator<void> > const>(std::mutex&, nav_msgs::Odometry_<std::allocator<void> > const&)343244
auto mrs_lib::set_mutexed<nav_msgs::Odometry_<std::allocator<void> > >(std::mutex&, nav_msgs::Odometry_<std::allocator<void> >, nav_msgs::Odometry_<std::allocator<void> >&)369227
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t&)370436
mrs_lib::KalmanFilter<6, 2, 2>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t&)370438
auto mrs_lib::set_mutexed<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::Float64ArrayStamped_<std::allocator<void> >, mrs_msgs::Float64ArrayStamped_<std::allocator<void> >&)508496
Eigen::Matrix<double, 2, 1, 0, 2, 1> const mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 1, 0, 2, 1> const>(std::mutex&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)557671
std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > mrs_lib::get_mutexed<std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > >(std::mutex&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > >&)659919
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)665622
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t&)751527
mrs_lib::KalmanFilter<3, 1, 1>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t&)752759
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)755880
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)893433
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)936212
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)969789
auto mrs_lib::set_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >, mrs_msgs::UavState_<std::allocator<void> >&)986295
double mrs_lib::get_mutexed<double>(std::mutex&, double&)1022999
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)1072011
mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&)2335966
mrs_uav_managers::estimation_manager::StateMachine::SMState_t const mrs_lib::get_mutexed<mrs_uav_managers::estimation_manager::StateMachine::SMState_t const>(std::mutex&, mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)2436193
mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&)2562522
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/mutex.h.func.html b/mrs_lib/include/mrs_lib/mutex.h.func.html new file mode 100644 index 0000000000..8ec78b88c6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func.html @@ -0,0 +1,400 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-04-25 05:42:57Functions:678083.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::tuple<Eigen::Matrix<double, -1, 1, 0, -1, 1>, double> mrs_lib::get_mutexed<Eigen::Matrix<double, -1, 1, 0, -1, 1>, double>(std::mutex&, Eigen::Matrix<double, -1, 1, 0, -1, 1>&, double&)70897
std::tuple<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > mrs_lib::get_mutexed<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::mutex&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&)228488
std::tuple<mrs_msgs::UavState_<std::allocator<void> >, double> mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> >, double>(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&, double&)313813
std::tuple<std::optional<double>, std::optional<double> > mrs_lib::get_mutexed<std::optional<double>, std::optional<double> >(std::mutex&, std::optional<double>&, std::optional<double>&)207796
std::tuple<bool, bool, ros::Time, ros::Time> mrs_lib::get_mutexed<bool, bool, ros::Time, ros::Time>(std::mutex&, bool&, bool&, ros::Time&, ros::Time&)12811
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)77111
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)17066
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)27925
std::tuple<double, double, double, double> mrs_lib::get_mutexed<double, double, double, double>(std::mutex&, double&, double&, double&, double&)0
std::tuple<double, double, double, double, double, double, double> mrs_lib::get_mutexed<double, double, double, double, double, double, double>(std::mutex&, double&, double&, double&, double&, double&, double&, double&)23295
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)15372
mrs_uav_managers::estimation_manager::StateMachine::SMState_t const mrs_lib::get_mutexed<mrs_uav_managers::estimation_manager::StateMachine::SMState_t const>(std::mutex&, mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)2436193
Eigen::Matrix<double, 1, 1, 0, 1, 1> const mrs_lib::get_mutexed<Eigen::Matrix<double, 1, 1, 0, 1, 1> const>(std::mutex&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)183558
Eigen::Matrix<double, 2, 1, 0, 2, 1> const mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 1, 0, 2, 1> const>(std::mutex&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)557671
Eigen::Matrix<double, 2, 2, 0, 2, 2> const mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> const>(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&)190532
mrs_msgs::Float64Stamped_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::Float64Stamped_<std::allocator<void> > const>(std::mutex&, mrs_msgs::Float64Stamped_<std::allocator<void> > const&)280826
mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const>(std::mutex&, mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)0
mrs_msgs::UavState_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > const>(std::mutex&, mrs_msgs::UavState_<std::allocator<void> > const&)184615
nav_msgs::Odometry_<std::allocator<void> > const mrs_lib::get_mutexed<nav_msgs::Odometry_<std::allocator<void> > const>(std::mutex&, nav_msgs::Odometry_<std::allocator<void> > const&)343244
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&)0
mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&)2335966
mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&)2562522
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const mrs_lib::get_mutexed<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const>(std::mutex&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)17984
std::vector<double, std::allocator<double> > const mrs_lib::get_mutexed<std::vector<double, std::allocator<double> > const>(std::mutex&, std::vector<double, std::allocator<double> > const&)317260
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)665622
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)755880
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)77111
mrs_uav_controllers::mpc_controller::Gains_t mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controller::Gains_t>(std::mutex&, mrs_uav_controllers::mpc_controller::Gains_t&)75201
mrs_uav_controllers::se3_controller::Gains_t mrs_lib::get_mutexed<mrs_uav_controllers::se3_controller::Gains_t>(std::mutex&, mrs_uav_controllers::se3_controller::Gains_t&)32830
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)136863
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)58400
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)39
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)893433
Eigen::Matrix<double, 2, 2, 0, 2, 2> mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
Eigen::Matrix<double, 3, 3, 0, 3, 3> mrs_lib::get_mutexed<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(std::mutex&, Eigen::Matrix<double, 3, 3, 0, 3, 3>&)296113
Eigen::Matrix<double, 6, 6, 0, 6, 6> mrs_lib::get_mutexed<Eigen::Matrix<double, 6, 6, 0, 6, 6> >(std::mutex&, Eigen::Matrix<double, 6, 6, 0, 6, 6>&)172034
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::get_mutexed<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::mutex&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&)246211
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)437
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)1107
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)731
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)263403
mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::SpeedTrackerCommand_<std::allocator<void> >&)0
mrs_msgs::MpcPredictionFullState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::MpcPredictionFullState_<std::allocator<void> > >(std::mutex&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> >&)70897
mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&)1
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)152028
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)969789
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
mrs_lib::KalmanFilter<3, 1, 1>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t&)752759
mrs_lib::KalmanFilter<6, 2, 2>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t&)370438
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::get_mutexed<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::mutex&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)81639
std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > mrs_lib::get_mutexed<std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >(std::mutex&, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&)224958
std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > mrs_lib::get_mutexed<std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > >(std::mutex&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > >&)659919
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)17162
double mrs_lib::get_mutexed<double>(std::mutex&, double&)1022999
int mrs_lib::get_mutexed<int>(std::mutex&, int&)8213
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)936212
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)100473
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controller::Gains_t>(std::mutex&, mrs_uav_controllers::mpc_controller::Gains_t, mrs_uav_controllers::mpc_controller::Gains_t&)14349
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controller::Gains_t>(std::mutex&, mrs_uav_controllers::se3_controller::Gains_t, mrs_uav_controllers::se3_controller::Gains_t&)4539
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)188
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)189
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)94
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 1, 0, 2, 1> >(std::mutex&, Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(std::mutex&, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(std::mutex&, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 1, 0, 6, 1> >(std::mutex&, Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Matrix<double, 6, 1, 0, 6, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 6, 0, 6, 6> >(std::mutex&, Eigen::Matrix<double, 6, 6, 0, 6, 6>, Eigen::Matrix<double, 6, 6, 0, 6, 6>&)0
auto mrs_lib::set_mutexed<mrs_msgs::Float64Stamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::Float64Stamped_<std::allocator<void> >, mrs_msgs::Float64Stamped_<std::allocator<void> >&)139277
auto mrs_lib::set_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)2138
auto mrs_lib::set_mutexed<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::Float64ArrayStamped_<std::allocator<void> >, mrs_msgs::Float64ArrayStamped_<std::allocator<void> >&)508496
auto mrs_lib::set_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)134
auto mrs_lib::set_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >, mrs_msgs::UavState_<std::allocator<void> >&)986295
auto mrs_lib::set_mutexed<nav_msgs::Odometry_<std::allocator<void> > >(std::mutex&, nav_msgs::Odometry_<std::allocator<void> >, nav_msgs::Odometry_<std::allocator<void> >&)369227
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t, mrs_lib::KalmanFilter<3, 1, 1>::statecov_t&)751527
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t, mrs_lib::KalmanFilter<6, 2, 2>::statecov_t&)370436
auto mrs_lib::set_mutexed<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::mutex&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)34471
auto mrs_lib::set_mutexed<std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >(std::mutex&, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&)206773
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)1072011
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/mutex.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/mutex.h.gcov.frameset.html new file mode 100644 index 0000000000..c07acea5c8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/mutex.h.gcov.html b/mrs_lib/include/mrs_lib/mutex.h.gcov.html new file mode 100644 index 0000000000..da00f11216 --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.gcov.html @@ -0,0 +1,260 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-04-25 05:42:57Functions:678083.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Defines helper routines for getting and setting variables under mutex locks
+       3             :      \author Tomas Baca - tomas.baca@fel.cvut.cz
+       4             :  */
+       5             : #ifndef MUTEX_H
+       6             : #define MUTEX_H
+       7             : 
+       8             : #include <iostream>
+       9             : #include <mutex>
+      10             : #include <tuple>
+      11             : 
+      12             : namespace mrs_lib
+      13             : {
+      14             : 
+      15             : /**
+      16             :  * @brief thread-safe getter and setter for values of variables (args)
+      17             :  *
+      18             :  * @tparam GetArgs types of the variables to get
+      19             :  * @tparam SetArgs types of the variables to set
+      20             :  * @param mut mutex which protects the variables
+      21             :  * @param get tuple of variable references to obtain the values from
+      22             :  * @param to_set tuple of variable references to set the new values from \p from_set
+      23             :  * @param from_set tuple of the new values to be set to \p to_set
+      24             :  *
+      25             :  * @return tuple of the values from \p get
+      26             :  */
+      27             : template <class... GetArgs, class... SetArgs>
+      28             : std::tuple<GetArgs...> get_set_mutexed(std::mutex& mut, std::tuple<GetArgs&...> get, std::tuple<SetArgs...> from_set, std::tuple<SetArgs&...> to_set) {
+      29             : 
+      30             :   std::scoped_lock lock(mut);
+      31             : 
+      32             :   std::tuple<GetArgs...> result = get;
+      33             :   to_set = from_set;
+      34             : 
+      35             :   return result;
+      36             : }
+      37             : 
+      38             : /**
+      39             :  * @brief thread-safe getter for values of variables (args)
+      40             :  *
+      41             :  * @tparam Args types of the variables
+      42             :  * @param mut mutex which protects the variables
+      43             :  * @param args variables to obtain the values from
+      44             :  *
+      45             :  * @return std::tuple of the values
+      46             :  */
+      47             : template <class... Args>
+      48      994574 : std::tuple<Args...> get_mutexed(std::mutex& mut, Args&... args) {
+      49             : 
+      50     1989148 :   std::scoped_lock lock(mut);
+      51             : 
+      52      994574 :   std::tuple result = std::tuple(args...);
+      53             : 
+      54     1989148 :   return result;
+      55             : }
+      56             : 
+      57             : 
+      58             : /**
+      59             :  * @brief thread-safe getter a value from a variable
+      60             :  *
+      61             :  * @tparam T type of the variable
+      62             :  * @param mut mutex which protects the variable
+      63             :  * @param arg variable to obtain the value from
+      64             :  *
+      65             :  * @return value of the variable
+      66             :  */
+      67             : template <class T>
+      68    18352800 : T get_mutexed(std::mutex& mut, T& arg) {
+      69             : 
+      70    30197669 :   std::scoped_lock lock(mut);
+      71             : 
+      72    36702803 :   return arg;
+      73             : }
+      74             : 
+      75             : /**
+      76             :  * @brief base case of the variadic template for set_mutexed()
+      77             :  *
+      78             :  * @tparam T variable type
+      79             :  * @param what value to set
+      80             :  * @param where reference to be set
+      81             :  */
+      82             : template <class T>
+      83             : void set_mutexed_impl(const T what, T& where) {
+      84             : 
+      85             :   where = what;
+      86             : }
+      87             : 
+      88             : /**
+      89             :  * @brief general case of the variadic template for set_mutexed()
+      90             :  *
+      91             :  * @tparam T type of the next variable to set
+      92             :  * @tparam Args types of the rest of the variables
+      93             :  * @param what value to set
+      94             :  * @param where reference to be set
+      95             :  * @param args the remaining arguments
+      96             :  */
+      97             : template <class T, class... Args>
+      98             : void set_mutexed_impl(const T what, T& where, Args... args) {
+      99             : 
+     100             :   where = what;
+     101             : 
+     102             :   set_mutexed_impl(args...);
+     103             : }
+     104             : 
+     105             : /**
+     106             :  * @brief thread-safe setter for a variable
+     107             :  *
+     108             :  * @tparam T type of the variable
+     109             :  * @param mut mutex to be locked
+     110             :  * @param what value to set
+     111             :  * @param where reference to be set
+     112             :  *
+     113             :  * @return
+     114             :  */
+     115             : template <class T>
+     116     4560617 : auto set_mutexed(std::mutex& mut, const T what, T& where) {
+     117             : 
+     118     8023501 :   std::scoped_lock lock(mut);
+     119             : 
+     120     4552769 :   where = what;
+     121             : 
+     122     9115913 :   return where;
+     123             : }
+     124             : 
+     125             : /**
+     126             :  * @brief thread-safe setter for multiple variables
+     127             :  *
+     128             :  * example:
+     129             :  *   set_mutexed(my_mutex_, a, a_, b, b_, c, c_);
+     130             :  *   where a, b, c are the values to be set
+     131             :  *         a_, b_, c_ are the variables to be updated
+     132             :  *
+     133             :  * @tparam Args types of the variables
+     134             :  * @param mut mutex to be locked
+     135             :  * @param args
+     136             :  *
+     137             :  * @return alternating list of values that were just set
+     138             :  */
+     139             : template <class... Args>
+     140             : auto set_mutexed(std::mutex& mut, Args&... args) {
+     141             : 
+     142             :   std::scoped_lock lock(mut);
+     143             : 
+     144             :   set_mutexed_impl(args...);
+     145             : 
+     146             :   return std::tuple(args...);
+     147             : }
+     148             : 
+     149             : /**
+     150             :  * @brief thread-safe setter for multiple variables
+     151             :  *
+     152             :  * example:
+     153             :  *   set_mutexed(mu_mutex, std::tuple(a, b, c), std::forward_as_tuple(a_, b_, c_));
+     154             :  *   where a, b, c are the values to be set
+     155             :  *         a_, b_, c_ are the updated variables
+     156             :  *
+     157             :  * @tparam Args types of the variables
+     158             :  * @param mut mutex to be locked
+     159             :  * @param from std::tuple of the values
+     160             :  * @param to std::tuple of reference to the variablaes
+     161             :  *
+     162             :  * @return
+     163             :  */
+     164             : template <class... Args>
+     165             : auto set_mutexed(std::mutex& mut, const std::tuple<Args...> from, std::tuple<Args&...> to) {
+     166             : 
+     167             :   std::scoped_lock lock(mut);
+     168             : 
+     169             :   to = from;
+     170             : 
+     171             :   return to;
+     172             : }
+     173             : 
+     174             : }  // namespace mrs_lib
+     175             : 
+     176             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/mutex.h.gcov.overview.html b/mrs_lib/include/mrs_lib/mutex.h.gcov.overview.html new file mode 100644 index 0000000000..3b0784eb80 --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.gcov.overview.html @@ -0,0 +1,64 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/mutex.h.gcov.png b/mrs_lib/include/mrs_lib/mutex.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..133eb4e0006b637a7fd96542303131f705ad7624 GIT binary patch literal 739 zcmV<90v!E`P)7|?WU-uAaUzR(5L09Z+v1q`*rF@#f&0R)y(WRk!}I}W4ls3S zL%{1Ah-H~+$P5I9W)UC~i!pJS8gUyCnEWLXN-S-F?731Y!x*~$p=}SxNLvHtscmz3 zdnjYjQ=VcfEb-TzK_{KiJfyVwBb4@R{GG|jKz?j8(ji$wT z+v2Pn*#hnGcfj%Ms%FFhv_e4+fARWEf=d9*B~XH{it9L3}0XmTdaewHKl}UMrts>XV8qkb%_aHnnf&%b%J{U zaqH-6bGE!dcuEYCvj;Rj}Sj-7_fxn*s z5=9BX&N_lt)}t> zUH8wBpq;4k!5VMQon&0V|0n?MI*SiZzwCQ(0f1f_VT*^_s;}_{0NZ~}1{wEWoj*#k V+z?^s1XKV3002ovPDHLkV1nIISQ-ET literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html b/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html new file mode 100644 index 0000000000..3750468f83 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html @@ -0,0 +1,392 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-04-25 05:42:57Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamLoader::loadParam2(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
XmlRpc::XmlRpcValue mrs_lib::ParamLoader::loadParam2<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixStatic<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>&)1
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic2<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadParamReusable<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
std::pair<Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1
bool mrs_lib::ParamLoader::loadMatrixKnown<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)2
bool mrs_lib::ParamLoader::loadMatrixKnown<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)2
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)2
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
mrs_lib::ParamLoader::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&)3
mrs_lib::ParamLoader::resetLoadedSuccessfully()3
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
mrs_lib::ParamLoader::printValue_recursive(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, unsigned int)5
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)5
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5
std::pair<Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
std::pair<XmlRpc::XmlRpcValue, bool> mrs_lib::ParamLoader::load<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
void mrs_lib::ParamLoader::printValue<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&)8
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)10
mrs_lib::ParamLoader::resetUniques()20
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)39
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)74
bool mrs_lib::ParamLoader::loadMatrixDynamic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)76
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > > const&)94
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > > const&, int, int)94
std::pair<Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)94
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixDynamic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)172
bool mrs_lib::ParamLoader::loadMatrixKnown<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)376
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)376
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::ParamLoader::loadParam2<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)470
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)648
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)652
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)752
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> > const&)1034
std::pair<std::vector<double, std::allocator<double> >, bool> mrs_lib::ParamLoader::load<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1034
bool mrs_lib::ParamLoader::loadParam<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&)1034
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1590
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1861
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2526
void mrs_lib::ParamLoader::printValue<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)3295
std::pair<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >, bool> mrs_lib::ParamLoader::load<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)3296
bool mrs_lib::ParamLoader::loadParam<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&)3296
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)3660
mrs_lib::ParamLoader::loadedSuccessfully()4098
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)4755
std::pair<int, bool> mrs_lib::ParamLoader::load<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4755
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)4755
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)5562
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10429
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)11045
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)15397
void mrs_lib::ParamLoader::printValue<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)15907
std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool> mrs_lib::ParamLoader::load<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)15907
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)16607
std::pair<bool, bool> mrs_lib::ParamLoader::load<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)16607
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)37727
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)38479
std::pair<double, bool> mrs_lib::ParamLoader::load<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)38479
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)80744
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/param_loader.h.func.html b/mrs_lib/include/mrs_lib/param_loader.h.func.html new file mode 100644 index 0000000000..0759d49baf --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func.html @@ -0,0 +1,392 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-04-25 05:42:57Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamLoader::loadParam2(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
XmlRpc::XmlRpcValue mrs_lib::ParamLoader::loadParam2<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::ParamLoader::loadParam2<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)470
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
mrs_lib::ParamLoader::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
void mrs_lib::ParamLoader::printValue<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)3295
void mrs_lib::ParamLoader::printValue<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)15907
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)16607
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)652
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> > const&)1034
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)38479
void mrs_lib::ParamLoader::printValue<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&)8
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)4755
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10429
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)648
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)10
mrs_lib::ParamLoader::resetUniques()20
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
bool mrs_lib::ParamLoader::loadMatrixKnown<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)376
bool mrs_lib::ParamLoader::loadMatrixKnown<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)2
bool mrs_lib::ParamLoader::loadMatrixKnown<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)2
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixStatic<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>&)1
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > > const&)94
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&)3
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)2
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)76
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > > const&, int, int)94
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic2<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadParamReusable<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)74
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
mrs_lib::ParamLoader::loadedSuccessfully()4098
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2526
mrs_lib::ParamLoader::printValue_recursive(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, unsigned int)5
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)80744
mrs_lib::ParamLoader::resetLoadedSuccessfully()3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)5
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)376
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
std::pair<Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1
std::pair<Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)94
std::pair<Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixDynamic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)172
std::pair<XmlRpc::XmlRpcValue, bool> mrs_lib::ParamLoader::load<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool> mrs_lib::ParamLoader::load<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)15907
std::pair<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >, bool> mrs_lib::ParamLoader::load<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)3296
std::pair<std::vector<double, std::allocator<double> >, bool> mrs_lib::ParamLoader::load<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1034
std::pair<bool, bool> mrs_lib::ParamLoader::load<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)16607
std::pair<double, bool> mrs_lib::ParamLoader::load<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)38479
std::pair<int, bool> mrs_lib::ParamLoader::load<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4755
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)15397
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)39
bool mrs_lib::ParamLoader::loadParam<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&)3296
bool mrs_lib::ParamLoader::loadParam<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&)1034
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)11045
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)5562
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)37727
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)752
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)4755
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1861
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1590
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)3660
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/param_loader.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/param_loader.h.gcov.frameset.html new file mode 100644 index 0000000000..2d925674cf --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/param_loader.h.gcov.html b/mrs_lib/include/mrs_lib/param_loader.h.gcov.html new file mode 100644 index 0000000000..12ec037b45 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.html @@ -0,0 +1,1452 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-04-25 05:42:57Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines ParamLoader - a convenience class for loading static ROS parameters.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef PARAM_LOADER_H
+       8             : #define PARAM_LOADER_H
+       9             : 
+      10             : #include <ros/ros.h>
+      11             : #include <string>
+      12             : #include <map>
+      13             : #include <unordered_set>
+      14             : #include <iostream>
+      15             : #include <Eigen/Dense>
+      16             : #include <std_msgs/ColorRGBA.h>
+      17             : #include <mrs_lib/param_provider.h>
+      18             : 
+      19             : namespace mrs_lib
+      20             : {
+      21             : 
+      22             : /*** ParamLoader CLASS //{ **/
+      23             : 
+      24             : /**
+      25             : * \brief Convenience class for loading parameters from rosparam server.
+      26             : *
+      27             : * The parameters can be loaded as compulsory. If a compulsory parameter is not found
+      28             : * on the rosparam server (e.g. because it is missing in the launchfile or the yaml config file),
+      29             : * an internal flag is set to false, indicating that the parameter loading procedure failed.
+      30             : * This flag can be checked using the loaded_successfully() method after all parameters were
+      31             : * attempted to be loaded (see usage example usage below).
+      32             : *
+      33             : * The loaded parameter names and corresponding values are printed to stdout by default
+      34             : * for user convenience. Special cases such as loading of Eigen matrices or loading
+      35             : * of std::vectors of various values are also provided.
+      36             : *
+      37             : * To load parameters into the `rosparam` server, use a launchfile prefferably.
+      38             : * See documentation of ROS launchfiles here: http://wiki.ros.org/roslaunch/XML.
+      39             : * Specifically, the `param` XML tag is used for loading parameters directly from the launchfile: http://wiki.ros.org/roslaunch/XML/param,
+      40             : * and the `rosparam` XML tag tag is used for loading parameters from a `yaml` file: http://wiki.ros.org/roslaunch/XML/rosparam.
+      41             : *
+      42             : */
+      43             : class ParamLoader
+      44             : {
+      45             : 
+      46             : private:
+      47             :   enum unique_t
+      48             :   {
+      49             :     UNIQUE = true,
+      50             :     REUSABLE = false
+      51             :   };
+      52             :   enum optional_t
+      53             :   {
+      54             :     OPTIONAL = true,
+      55             :     COMPULSORY = false
+      56             :   };
+      57             :   enum swap_t
+      58             :   {
+      59             :     SWAP = true,
+      60             :     NO_SWAP = false
+      61             :   };
+      62             : 
+      63             :   template <typename T>
+      64             :   using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+      65             : 
+      66             : private:
+      67             :   bool m_load_successful, m_print_values;
+      68             :   std::string m_node_name;
+      69             :   std::string m_prefix;
+      70             :   const ros::NodeHandle& m_nh;
+      71             :   mrs_lib::ParamProvider m_pp;
+      72             :   std::unordered_set<std::string> m_loaded_params;
+      73             : 
+      74             :   /* printing helper functions //{ */
+      75             :   /* printError and print_warning functions //{*/
+      76           7 :   void printError(const std::string& str)
+      77             :   {
+      78           7 :     if (m_node_name.empty())
+      79           7 :       ROS_ERROR_STREAM(str);
+      80             :     else
+      81           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: " << str);
+      82           7 :   }
+      83           1 :   void print_warning(const std::string& str)
+      84             :   {
+      85           1 :     if (m_node_name.empty())
+      86           1 :       ROS_WARN_STREAM(str);
+      87             :     else
+      88           0 :       ROS_WARN_STREAM("[" << m_node_name << "]: " << str);
+      89           1 :   }
+      90             :   //}
+      91             : 
+      92             :   /* printValue function and overloads //{ */
+      93             : 
+      94             :   template <typename T>
+      95       75748 :   void printValue(const std::string& name, const T& value)
+      96             :   {
+      97       75748 :     if (m_node_name.empty())
+      98       42314 :       std::cout << "\t" << name << ":\t" << value << std::endl;
+      99             :     else
+     100       33434 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << value);
+     101       75748 :   }
+     102             : 
+     103             :   template <typename T>
+     104        4329 :   void printValue(const std::string& name, const std::vector<T>& value)
+     105             :   {
+     106        8658 :     std::stringstream strstr;
+     107        4329 :     if (m_node_name.empty())
+     108        2100 :       strstr << "\t";
+     109        4329 :     strstr << name << ":\t";
+     110        4329 :     size_t it = 0;
+     111       15133 :     for (const auto& elem : value)
+     112             :     {
+     113       10804 :       strstr << elem;
+     114       10804 :       if (it < value.size() - 1)
+     115        6956 :         strstr << ", ";
+     116       10804 :       it++;
+     117             :     }
+     118        4329 :     if (m_node_name.empty())
+     119        2100 :       std::cout << strstr.str() << std::endl;
+     120             :     else
+     121        2229 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << strstr.str());
+     122        4329 :   }
+     123             : 
+     124             :   template <typename T1, typename T2>
+     125             :   void printValue(const std::string& name, const std::map<T1, T2>& value)
+     126             :   {
+     127             :     std::stringstream strstr;
+     128             :     if (m_node_name.empty())
+     129             :       strstr << "\t";
+     130             :     strstr << name << ":" << std::endl;
+     131             :     size_t it = 0;
+     132             :     for (const auto& pair : value)
+     133             :     {
+     134             :       strstr << pair.first << " = " << pair.second;
+     135             :       if (it < value.size() - 1)
+     136             :         strstr << std::endl;
+     137             :       it++;
+     138             :     }
+     139             :     if (m_node_name.empty())
+     140             :       std::cout << strstr.str() << std::endl;
+     141             :     else
+     142             :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << strstr.str());
+     143             :   }
+     144             : 
+     145             :   template <typename T>
+     146         660 :   void printValue(const std::string& name, const MatrixX<T>& value)
+     147             :   {
+     148        1320 :     std::stringstream strstr;
+     149             :     /* const Eigen::IOFormat fmt(4, 0, ", ", "\n", "\t\t[", "]"); */
+     150             :     /* strstr << value.format(fmt); */
+     151        1980 :     const Eigen::IOFormat fmt;
+     152         660 :     strstr << value.format(fmt);
+     153         660 :     if (m_node_name.empty())
+     154          96 :       std::cout << "\t" << name << ":\t" << std::endl << strstr.str() << std::endl;
+     155             :     else
+     156         564 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':" << std::endl << strstr.str());
+     157         660 :   }
+     158             : 
+     159           5 :   std::string printValue_recursive(const std::string& name, XmlRpc::XmlRpcValue& value, unsigned depth = 0)
+     160             :   {
+     161          10 :     std::stringstream strstr;
+     162           8 :     for (unsigned it = 0; it < depth; it++)
+     163           3 :       strstr << "\t";
+     164           5 :     strstr << name << ":";
+     165           5 :     switch (value.getType())
+     166             :     {
+     167           1 :       case XmlRpc::XmlRpcValue::TypeArray:
+     168             :         {
+     169           2 :           for (int it = 0; it < value.size(); it++)
+     170             :           {
+     171           1 :             strstr << std::endl;
+     172           2 :             const std::string name = "[" + std::to_string(it) + "]";
+     173           1 :             strstr << printValue_recursive(name, value[it], depth+1);
+     174             :           }
+     175           1 :           break;
+     176             :         }
+     177           1 :       case XmlRpc::XmlRpcValue::TypeStruct:
+     178             :         {
+     179           1 :           int it = 0;
+     180           2 :           for (auto& pair : value)
+     181             :           {
+     182           1 :             strstr << std::endl;
+     183           1 :             strstr << printValue_recursive(pair.first, pair.second, depth+1);
+     184           1 :             it++;
+     185             :           }
+     186           1 :           break;
+     187             :         }
+     188           3 :       default:
+     189             :         {
+     190           3 :           strstr << "\t" << value;
+     191           3 :           break;
+     192             :         }
+     193             :     }
+     194          10 :     return strstr.str();
+     195             :   }
+     196             : 
+     197           3 :   void printValue(const std::string& name, XmlRpc::XmlRpcValue& value)
+     198             :   {
+     199           6 :     const std::string txt = printValue_recursive(name, value);
+     200           3 :     if (m_node_name.empty())
+     201           3 :       std::cout << txt << std::endl;
+     202             :     else
+     203           0 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << txt);
+     204           3 :   }
+     205             : 
+     206             :   //}
+     207             :   
+     208           5 :   std::string resolved(const std::string& param_name)
+     209             :   {
+     210           5 :     return m_nh.resolveName(param_name);
+     211             :   }
+     212             :   //}
+     213             : 
+     214             :   /* check_duplicit_loading checks whether the parameter was already loaded - returns true if yes //{ */
+     215       80744 :   bool check_duplicit_loading(const std::string& name)
+     216             :   {
+     217       80744 :     if (m_loaded_params.count(name))
+     218             :     {
+     219           2 :       printError(std::string("Tried to load parameter ") + name + std::string(" twice"));
+     220           2 :       m_load_successful = false;
+     221           2 :       return true;
+     222             :     } else
+     223             :     {
+     224       80743 :       return false;
+     225             :     }
+     226             :   }
+     227             :   //}
+     228             : 
+     229             :   /* helper functions for loading dynamic Eigen matrices //{ */
+     230             :   // loadMatrixX helper function for loading dynamic Eigen matrices //{
+     231             :   template <typename T>
+     232         658 :   std::pair<MatrixX<T>, bool> loadMatrixX(const std::string& name, const MatrixX<T>& default_value, int rows, int cols = Eigen::Dynamic, optional_t optional = OPTIONAL, unique_t unique = UNIQUE, swap_t swap = NO_SWAP, bool printValues = true)
+     233             :   {
+     234        1316 :     const std::string name_prefixed = m_prefix + name;
+     235        1316 :     MatrixX<T> loaded = default_value;
+     236         658 :     bool used_rosparam_value = false;
+     237             :     // first, check if the user already tried to load this parameter
+     238         658 :     if (unique && check_duplicit_loading(name_prefixed))
+     239           0 :       return {loaded, used_rosparam_value};
+     240             : 
+     241             :     // this function only accepts dynamic columns (you can always transpose the matrix afterward)
+     242         658 :     if (rows < 0)
+     243             :     {
+     244             :       // if the parameter was compulsory, alert the user and set the flag
+     245           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed));
+     246           0 :       m_load_successful = false;
+     247           0 :       return {loaded, used_rosparam_value};
+     248             :     }
+     249         658 :     const bool expect_zero_matrix = rows == 0;
+     250         658 :     if (expect_zero_matrix)
+     251             :     {
+     252           1 :       if (cols > 0)
+     253             :       {
+     254           0 :         printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed) + ". One dimension indicates zero matrix, but other expects non-zero.");
+     255           0 :         m_load_successful = false;
+     256           0 :         return {loaded, used_rosparam_value};
+     257             :       }
+     258             :     }
+     259             : 
+     260         658 :     bool cur_load_successful = true;
+     261         658 :     bool check_size_exact = true;
+     262         658 :     if (cols <= 0)  // this means that the cols dimension is dynamic or a zero matrix is expected
+     263         170 :       check_size_exact = false;
+     264             : 
+     265        1316 :     std::vector<T> tmp_vec;
+     266             :     // try to load the parameter
+     267         658 :     const bool success = m_pp.getParam(name_prefixed, tmp_vec);
+     268             :     // check if the loaded vector has correct length
+     269         658 :     bool correct_size = (int)tmp_vec.size() == rows * cols;
+     270         658 :     if (!check_size_exact && !expect_zero_matrix)
+     271         169 :       correct_size = (int)tmp_vec.size() % rows == 0;  // if the cols dimension is dynamic, the size just has to be divisable by rows
+     272             : 
+     273         658 :     if (success && correct_size)
+     274             :     {
+     275             :       // if successfully loaded, everything is in order
+     276             :       // transform the vector to the matrix
+     277         555 :       if (cols <= 0 && rows > 0)
+     278         169 :         cols = tmp_vec.size() / rows;
+     279         555 :       if (swap)
+     280          74 :         std::swap(rows, cols);
+     281         555 :       loaded = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>, Eigen::Unaligned>(tmp_vec.data(), rows, cols);
+     282         555 :       used_rosparam_value = true;
+     283             :     } else
+     284             :     {
+     285         103 :       if (success && !correct_size)
+     286             :       {
+     287             :         // warn the user that this parameter was not successfully loaded because of wrong vector length (might be an oversight)
+     288           3 :         std::string warning =
+     289             :             std::string("Matrix parameter ") + name_prefixed
+     290             :             + std::string(" could not be loaded because the vector has a wrong length " + std::to_string(tmp_vec.size()) + " instead of expected ");
+     291             :         // process the message correctly based on whether the loaded matrix should be dynamic or static
+     292           1 :         if (cols <= 0)  // for dynamic matrices
+     293           0 :           warning = warning + std::string("number divisible by ") + std::to_string(rows);
+     294             :         else  // for static matrices
+     295           1 :           warning = warning + std::to_string(rows * cols);
+     296           1 :         print_warning(warning);
+     297             :       }
+     298             :       // if it was not loaded, the default value is used (set at the beginning of the function)
+     299         103 :       if (!optional)
+     300             :       {
+     301             :         // if the parameter was compulsory, alert the user and set the flag
+     302           3 :         printError(std::string("Could not load non-optional parameter ") + resolved(name_prefixed));
+     303           3 :         cur_load_successful = false;
+     304             :       }
+     305             :     }
+     306             : 
+     307             :     // check if load was a success
+     308         658 :     if (cur_load_successful)
+     309             :     {
+     310         655 :       if (m_print_values && printValues)
+     311         650 :         printValue(name_prefixed, loaded);
+     312         655 :       m_loaded_params.insert(name_prefixed);
+     313             :     } else
+     314             :     {
+     315           3 :       m_load_successful = false;
+     316             :     }
+     317             :     // finally, return the resulting value
+     318         658 :     return {loaded, used_rosparam_value};
+     319             :   }
+     320             :   //}
+     321             : 
+     322             :   /* loadMatrixStatic_internal helper function for loading static Eigen matrices //{ */
+     323             :   template <int rows, int cols, typename T>
+     324         101 :   std::pair<Eigen::Matrix<T, rows, cols>, bool> loadMatrixStatic_internal(const std::string& name, const Eigen::Matrix<T, rows, cols>& default_value, optional_t optional, unique_t unique)
+     325             :   {
+     326         303 :     const auto [dynamic, loaded_ok] = loadMatrixX(name, MatrixX<T>(default_value), rows, cols, optional, unique, NO_SWAP);
+     327         202 :     return {dynamic, loaded_ok};
+     328             :   }
+     329             :   //}
+     330             : 
+     331             :   /* loadMatrixKnown_internal helper function for loading EigenXd matrices with known dimensions //{ */
+     332             :   template <typename T>
+     333         380 :   std::pair<MatrixX<T>, bool> loadMatrixKnown_internal(const std::string& name, const MatrixX<T>& default_value, int rows, int cols, optional_t optional, unique_t unique)
+     334             :   {
+     335         760 :     MatrixX<T> loaded = default_value;
+     336             :     // first, check that at least one dimension is set
+     337         380 :     if (rows <= 0 || cols <= 0)
+     338             :     {
+     339           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name) + std::string(" (use loadMatrixDynamic?)"));
+     340           0 :       m_load_successful = false;
+     341           0 :       return {loaded, false};
+     342             :     }
+     343             : 
+     344         380 :     return loadMatrixX(name, default_value, rows, cols, optional, unique, NO_SWAP);
+     345             :   }
+     346             :   //}
+     347             : 
+     348             :   /* loadMatrixDynamic_internal helper function for loading Eigen matrices with one dynamic (unspecified) dimension //{ */
+     349             :   template <typename T>
+     350         172 :   std::pair<MatrixX<T>, bool> loadMatrixDynamic_internal(const std::string& name, const MatrixX<T>& default_value, int rows, int cols, optional_t optional, unique_t unique)
+     351             :   {
+     352         344 :     MatrixX<T> loaded = default_value;
+     353             : 
+     354             :     // next, check that at least one dimension is set
+     355         172 :     if (rows <= 0 && cols <= 0)
+     356             :     {
+     357           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name) + std::string(" (at least one dimension must be specified)"));
+     358           0 :       m_load_successful = false;
+     359           0 :       return {loaded, false};
+     360             :     }
+     361             : 
+     362         172 :     swap_t swap = NO_SWAP;
+     363         172 :     if (rows <= 0)
+     364             :     {
+     365          74 :       std::swap(rows, cols);
+     366          74 :       swap = SWAP;
+     367             :     }
+     368         172 :     return loadMatrixX(name, default_value, rows, cols, optional, unique, swap);
+     369             :   }
+     370             :   //}
+     371             :   //}
+     372             : 
+     373             :   /* loadMatrixArray_internal helper function for loading an array of EigenXd matrices with known dimensions //{ */
+     374             :   template <typename T>
+     375           5 :   std::vector<MatrixX<T>> loadMatrixArray_internal(const std::string& name, const std::vector<MatrixX<T>>& default_value, optional_t optional, unique_t unique)
+     376             :   {
+     377          10 :     const std::string name_prefixed = m_prefix + name;
+     378             :     int rows;
+     379          10 :     std::vector<int> cols;
+     380           5 :     bool success = true;
+     381           5 :     success = success && m_pp.getParam(name_prefixed + "/rows", rows);
+     382           5 :     success = success && m_pp.getParam(name_prefixed + "/cols", cols);
+     383             : 
+     384          10 :     std::vector<MatrixX<T>> loaded;
+     385           5 :     loaded.reserve(cols.size());
+     386             : 
+     387           5 :     int total_cols = 0;
+     388             :     /* check correctness of loaded parameters so far calculate the total dimension //{ */
+     389             : 
+     390           5 :     if (!success)
+     391             :     {
+     392           0 :       printError(std::string("Failed to load ") + resolved(name_prefixed) + std::string("/rows or ") + resolved(name_prefixed) + std::string("/cols"));
+     393           0 :       m_load_successful = false;
+     394           0 :       return default_value;
+     395             :     }
+     396           5 :     if (rows < 0)
+     397             :     {
+     398           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed) + std::string(" (rows and cols must be >= 0)"));
+     399           0 :       m_load_successful = false;
+     400           0 :       return default_value;
+     401             :     }
+     402          15 :     for (const auto& col : cols)
+     403             :     {
+     404          10 :       if (col < 0)
+     405             :       {
+     406           0 :         printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed) + std::string(" (rows and cols must be >= 0)"));
+     407           0 :         m_load_successful = false;
+     408           0 :         return default_value;
+     409             :       }
+     410          10 :       total_cols += col;
+     411             :     }
+     412             :     
+     413             :     //}
+     414             : 
+     415          15 :     const auto [loaded_matrix, loaded_ok] = loadMatrixX(name + "/data", MatrixX<T>(), rows, total_cols, optional, unique, NO_SWAP, false);
+     416             :     /* std::cout << "loaded_matrix: " << loaded_matrix << std::endl; */
+     417             :     /* std::cout << "loaded_matrix: " << loaded_matrix.rows() << "x" << loaded_matrix.cols() << std::endl; */
+     418             :     /* std::cout << "expected dims: " << rows << "x" << total_cols << std::endl; */
+     419           5 :     if (loaded_matrix.rows() != rows || loaded_matrix.cols() != total_cols)
+     420             :     {
+     421           0 :       m_load_successful = false;
+     422           0 :       return default_value;
+     423             :     }
+     424             : 
+     425           5 :     int cols_loaded = 0;
+     426          15 :     for (unsigned it = 0; it < cols.size(); it++)
+     427             :     {
+     428          10 :       const int cur_cols = cols.at(it);
+     429          10 :       const MatrixX<T> cur_mat = loaded_matrix.block(0, cols_loaded, rows, cur_cols);
+     430             :       /* std::cout << "cur_mat: " << cur_mat << std::endl; */
+     431          10 :       loaded.push_back(cur_mat);
+     432          10 :       cols_loaded += cur_cols;
+     433          10 :       printValue(name_prefixed + "/matrix#" + std::to_string(it), cur_mat);
+     434             :     }
+     435           5 :     return loaded;
+     436             :   }
+     437             :   //}
+     438             : 
+     439             :   /* load helper function for generic types //{ */
+     440             :   // This function tries to load a parameter with name 'name' and a default value.
+     441             :   // You can use the flag 'optional' to not throw a ROS_ERROR when the parameter
+     442             :   // cannot be loaded and the flag 'printValues' to set whether the loaded
+     443             :   // value and name of the parameter should be printed to cout.
+     444             :   // If 'optional' is set to false and the parameter could not be loaded,
+     445             :   // the flag 'load_successful' is set to false and a ROS_ERROR message
+     446             :   // is printer.
+     447             :   // If 'unique' flag is set to false then the parameter is not checked
+     448             :   // for being loaded twice.
+     449             :   // Returns a tuple, containing either the loaded or the default value and a bool,
+     450             :   // indicating if the value was loaded (true) or the default value was used (false).
+     451             :   template <typename T>
+     452       80084 :   std::pair<T, bool> load(const std::string& name, const T& default_value, optional_t optional = OPTIONAL, unique_t unique = UNIQUE)
+     453             :   {
+     454      160168 :     const std::string name_prefixed = m_prefix + name;
+     455      100327 :     T loaded = default_value;
+     456       80084 :     if (unique && check_duplicit_loading(name_prefixed))
+     457           2 :       return {loaded, false};
+     458             : 
+     459       80082 :     bool cur_load_successful = true;
+     460             :     // try to load the parameter
+     461       80082 :     const bool success = m_pp.getParam(name_prefixed, loaded);
+     462       80082 :     if (!success)
+     463             :     {
+     464             :       // if it was not loaded, set the default value
+     465        1438 :       loaded = default_value;
+     466        1438 :       if (!optional)
+     467             :       {
+     468             :         // if the parameter was compulsory, alert the user and set the flag
+     469           2 :         printError(std::string("Could not load non-optional parameter ") + resolved(name_prefixed));
+     470           2 :         cur_load_successful = false;
+     471             :       }
+     472             :     }
+     473             : 
+     474       80082 :     if (cur_load_successful)
+     475             :     {
+     476             :       // everything is fine and just print the name_prefixed and value if required
+     477       80080 :       if (m_print_values)
+     478       80080 :         printValue(name_prefixed, loaded);
+     479             :       // mark the param name_prefixed as successfully loaded
+     480       80080 :       m_loaded_params.insert(name_prefixed);
+     481             :     } else
+     482             :     {
+     483           2 :       m_load_successful = false;
+     484             :     }
+     485             :     // finally, return the resulting value
+     486       80082 :     return {loaded, success};
+     487             :   }
+     488             :   //}
+     489             : 
+     490             : public:
+     491             :   /*!
+     492             :     * \brief Main constructor.
+     493             :     *
+     494             :     * \param nh            The parameters will be loaded from rosparam using this node handle.
+     495             :     * \param printValues  If true, the loaded values will be printed to stdout using std::cout or ROS_INFO if node_name is not empty.
+     496             :     * \param node_name     Optional node name used when printing the loaded values or loading errors.
+     497             :     */
+     498        3660 :   ParamLoader(const ros::NodeHandle& nh, bool printValues = true, std::string_view node_name = std::string())
+     499        3660 :       : m_load_successful(true),
+     500             :         m_print_values(printValues),
+     501             :         m_node_name(node_name),
+     502             :         m_nh(nh),
+     503        3660 :         m_pp(nh, m_node_name)
+     504             :   {
+     505             :     /* std::cout << "Initialized1 ParamLoader for node " << node_name << std::endl; */
+     506        3660 :   }
+     507             : 
+     508             :   /* Constructor overloads //{ */
+     509             :   /*!
+     510             :     * \brief Convenience overload to enable writing ParamLoader pl(nh, node_name);
+     511             :     *
+     512             :     * \param nh            The parameters will be loaded from rosparam using this node handle.
+     513             :     * \param node_name     Optional node name used when printing the loaded values or loading errors.
+     514             :     */
+     515        1590 :   ParamLoader(const ros::NodeHandle& nh, std::string_view node_name)
+     516        1590 :       : ParamLoader(nh, true, node_name)
+     517             :   {
+     518             :     /* std::cout << "Initialized2 ParamLoader for node " << node_name << std::endl; */
+     519        1590 :   }
+     520             : 
+     521             :   /*!
+     522             :     * \brief Convenience overload to enable writing ParamLoader pl(nh, "node_name");
+     523             :     *
+     524             :     * \param nh            The parameters will be loaded from rosparam using this node handle.
+     525             :     * \param node_name     Optional node name used when printing the loaded values or loading errors.
+     526             :     */
+     527             :   ParamLoader(const std::string& filepath, const ros::NodeHandle& nh)
+     528             :     : ParamLoader(nh, "none")
+     529             :   {
+     530             :     YAML::Node node = YAML::Load(filepath);
+     531             :   }
+     532             :   //}
+     533             : 
+     534             :   /* setPrefix function //{ */
+     535             :   
+     536             :   /*!
+     537             :     * \brief All loaded parameters will be prefixed with this string.
+     538             :     *
+     539             :     * \param prefix  the prefix to be applied to all loaded parameters from now on.
+     540             :     */
+     541        1861 :   void setPrefix(const std::string& prefix)
+     542             :   {
+     543        1861 :     m_prefix = prefix;
+     544        1861 :   }
+     545             :   
+     546             :   //}
+     547             : 
+     548             :   /* getPrefix function //{ */
+     549             :   
+     550             :   /*!
+     551             :     * \brief Returns the current parameter name prefix.
+     552             :     *
+     553             :     * \return the current prefix to be applied to the loaded parameters.
+     554             :     */
+     555             :   std::string getPrefix()
+     556             :   {
+     557             :     return m_prefix;
+     558             :   }
+     559             :   
+     560             :   //}
+     561             : 
+     562             :   /* addYamlFile() function //{ */
+     563             :   
+     564             :   /*!
+     565             :     * \brief Adds the specified file as a source of static parameters.
+     566             :     *
+     567             :     * \param filepath The full path to the yaml file to be loaded.
+     568             :     * \return true if loading and parsing the file was successful, false otherwise.
+     569             :     */
+     570       10429 :   bool addYamlFile(const std::string& filepath)
+     571             :   {
+     572       10429 :     return m_pp.addYamlFile(filepath);
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* addYamlFileFromParam() function //{ */
+     577             :   
+     578             :   /*!
+     579             :     * \brief Loads a filepath from a parameter loads that file as a YAML.
+     580             :     *
+     581             :     * \param param_name Name of the parameter from which to load the YAML filename to be loaded.
+     582             :     * \return true      if loading and parsing the file was successful, false otherwise.
+     583             :     */
+     584        2526 :   bool addYamlFileFromParam(const std::string& param_name)
+     585             :   {
+     586        5052 :     std::string filepath;
+     587        2526 :     if (!loadParam(param_name, filepath))
+     588           0 :       return false;
+     589        2526 :     return m_pp.addYamlFile(filepath);
+     590             :   }
+     591             :   //}
+     592             : 
+     593             :   /* loadedSuccessfully function //{ */
+     594             :   /*!
+     595             :     * \brief Indicates whether all compulsory parameters were successfully loaded.
+     596             :     *
+     597             :     * \return false if any compulsory parameter was not loaded (is not present at rosparam server). Otherwise returns true.
+     598             :     */
+     599        4098 :   bool loadedSuccessfully()
+     600             :   {
+     601        4098 :     return m_load_successful;
+     602             :   }
+     603             :   //}
+     604             : 
+     605             :   /* resetLoadedSuccessfully function //{ */
+     606             :   /*!
+     607             :     * \brief Resets the loadedSuccessfully flag back to true.
+     608             :     */
+     609           3 :   void resetLoadedSuccessfully()
+     610             :   {
+     611           3 :     m_load_successful = true;
+     612           3 :   }
+     613             :   //}
+     614             : 
+     615             :   /* resetUniques function //{ */
+     616             :   /*!
+     617             :     * \brief Resets the list of already loaded parameter names used when checking for uniqueness.
+     618             :     */
+     619          20 :   void resetUniques()
+     620             :   {
+     621          20 :     m_loaded_params.clear();
+     622          20 :   }
+     623             :   //}
+     624             : 
+     625             :   /* loadParam function for optional parameters //{ */
+     626             :   /*!
+     627             :     * \brief Loads a parameter from the rosparam server with a default value.
+     628             :     *
+     629             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     630             :     * the default value is used.
+     631             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     632             :     *
+     633             :     * \param name          Name of the parameter in the rosparam server.
+     634             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     635             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     636             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     637             :     */
+     638             :   template <typename T>
+     639        6355 :   bool loadParam(const std::string& name, T& out_value, const T& default_value)
+     640             :   {
+     641        6355 :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     642        6355 :     out_value = ret;
+     643        6396 :     return success;
+     644             :   }
+     645             :   /*!
+     646             :     * \brief Loads a parameter from the rosparam server with a default value.
+     647             :     *
+     648             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     649             :     * the default value is used.
+     650             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     651             :     *
+     652             :     * \param name          Name of the parameter in the rosparam server.
+     653             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     654             :     * \return              The loaded parameter value.
+     655             :     */
+     656             :   template <typename T>
+     657         471 :   T loadParam2(const std::string& name, const T& default_value)
+     658             :   {
+     659         942 :     const auto loaded = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     660         942 :     return loaded.first;
+     661             :   }
+     662             :   /*!
+     663             :     * \brief Loads a parameter from the rosparam server with a default value.
+     664             :     *
+     665             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     666             :     * the default value is used.
+     667             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     668             :     *
+     669             :     * \param name          Name of the parameter in the rosparam server.
+     670             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     671             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     672             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     673             :     */
+     674             :   template <typename T>
+     675             :   bool loadParamReusable(const std::string& name, T& out_value, const T& default_value)
+     676             :   {
+     677             :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, REUSABLE);
+     678             :     out_value = ret;
+     679             :     return success;
+     680             :   }
+     681             :   /*!
+     682             :     * \brief Loads an optional reusable parameter from the rosparam server.
+     683             :     *
+     684             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     685             :     * the default value is used.
+     686             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     687             :     *
+     688             :     * \param name          Name of the parameter in the rosparam server.
+     689             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     690             :     * \return              The loaded parameter value.
+     691             :     */
+     692             :   template <typename T>
+     693             :   T loadParamReusable2(const std::string& name, const T& default_value)
+     694             :   {
+     695             :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, REUSABLE);
+     696             :     return ret;
+     697             :   }
+     698             :   //}
+     699             : 
+     700             :   /* loadParam function for compulsory parameters //{ */
+     701             :   /*!
+     702             :     * \brief Loads a compulsory parameter from the rosparam server.
+     703             :     *
+     704             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     705             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     706             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     707             :     *
+     708             :     * \param name          Name of the parameter in the rosparam server.
+     709             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     710             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     711             :     */
+     712             :   template <typename T>
+     713       73257 :   bool loadParam(const std::string& name, T& out_value)
+     714             :   {
+     715       92987 :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
+     716       73257 :     out_value = ret;
+     717       92987 :     return success;
+     718             :   }
+     719             :   /*!
+     720             :     * \brief Loads a compulsory parameter from the rosparam server.
+     721             :     *
+     722             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     723             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     724             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     725             :     *
+     726             :     * \param name          Name of the parameter in the rosparam server.
+     727             :     * \return              The loaded parameter value.
+     728             :     */
+     729             :   template <typename T>
+     730             :   T loadParam2(const std::string& name)
+     731             :   {
+     732             :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
+     733             :     return ret;
+     734             :   }
+     735             :   /*!
+     736             :     * \brief Loads a compulsory parameter from the rosparam server.
+     737             :     *
+     738             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     739             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     740             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     741             :     *
+     742             :     * \param name          Name of the parameter in the rosparam server.
+     743             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     744             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     745             :     */
+     746             :   template <typename T>
+     747           1 :   bool loadParamReusable(const std::string& name, T& out_value)
+     748             :   {
+     749           2 :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, REUSABLE);
+     750           1 :     out_value = ret;
+     751           2 :     return success;
+     752             :   }
+     753             :   /*!
+     754             :     * \brief Loads a compulsory parameter from the rosparam server.
+     755             :     *
+     756             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     757             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     758             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     759             :     *
+     760             :     * \param name          Name of the parameter in the rosparam server.
+     761             :     * \return              The loaded parameter value.
+     762             :     */
+     763             :   template <typename T>
+     764             :   T loadParamReusable2(const std::string& name)
+     765             :   {
+     766             :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, REUSABLE);
+     767             :     return ret;
+     768             :   }
+     769             :   //}
+     770             : 
+     771             :   /* loadParam specializations for ros::Duration type //{ */
+     772             : 
+     773             :   /*!
+     774             :     * \brief An overload for loading ros::Duration.
+     775             :     *
+     776             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+     777             :     *
+     778             :     * \param name          Name of the parameter in the rosparam server.
+     779             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     780             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     781             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     782             :     */
+     783             :   bool loadParam(const std::string& name, ros::Duration& out, const ros::Duration& default_value)
+     784             :   {
+     785             :     double secs;
+     786             :     const bool ret = loadParam<double>(name, secs, default_value.toSec());
+     787             :     out = ros::Duration(secs);
+     788             :     return ret;
+     789             :   }
+     790             : 
+     791             :   /*!
+     792             :     * \brief An overload for loading ros::Duration.
+     793             :     *
+     794             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+     795             :     *
+     796             :     * \param name          Name of the parameter in the rosparam server.
+     797             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     798             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     799             :     */
+     800             :   bool loadParam(const std::string& name, ros::Duration& out)
+     801             :   {
+     802             :     double secs;
+     803             :     const bool ret = loadParam<double>(name, secs);
+     804             :     out = ros::Duration(secs);
+     805             :     return ret;
+     806             :   }
+     807             :   
+     808             :   //}
+     809             : 
+     810             :   /* loadParam specializations for std_msgs::ColorRGBA type //{ */
+     811             : 
+     812             :   /*!
+     813             :     * \brief An overload for loading std_msgs::ColorRGBA.
+     814             :     *
+     815             :     * The color will be loaded as several \p double -typed variables, representing the R, G, B and A color elements.
+     816             :     *
+     817             :     * \param name          Name of the parameter in the rosparam server.
+     818             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     819             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     820             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     821             :     */
+     822             :   bool loadParam(const std::string& name, std_msgs::ColorRGBA& out, const std_msgs::ColorRGBA& default_value = {})
+     823             :   {
+     824             :     std_msgs::ColorRGBA res;
+     825             :     bool ret = true;
+     826             :     ret = ret & loadParam(name+"/r", res.r, default_value.r);
+     827             :     ret = ret & loadParam(name+"/g", res.g, default_value.g);
+     828             :     ret = ret & loadParam(name+"/b", res.b, default_value.b);
+     829             :     ret = ret & loadParam(name+"/a", res.a, default_value.a);
+     830             :     if (ret)
+     831             :       out = res;
+     832             :     return ret;
+     833             :   }
+     834             : 
+     835             :   /*!
+     836             :     * \brief An overload for loading std_msgs::ColorRGBA.
+     837             :     *
+     838             :     * The color will be loaded as several \p double -typed variables, representing the R, G, B and A color elements.
+     839             :     *
+     840             :     * \param name          Name of the parameter in the rosparam server.
+     841             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     842             :     * \return              The loaded parameter value.
+     843             :     */
+     844             :   std_msgs::ColorRGBA loadParam2(const std::string& name, const std_msgs::ColorRGBA& default_value = {})
+     845             :   {
+     846             :     std_msgs::ColorRGBA ret;
+     847             :     loadParam(name, ret, default_value);
+     848             :     return ret;
+     849             :   }
+     850             : 
+     851             :   //}
+     852             : 
+     853             :   /* loadParam specializations for XmlRpc::Value type //{ */
+     854             : 
+     855             :   /*!
+     856             :     * \brief An overload for loading an optional XmlRpc::XmlRpcValue.
+     857             :     *
+     858             :     * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+     859             :     * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+     860             :     * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+     861             :     *
+     862             :     * \param name          Name of the parameter in the rosparam server.
+     863             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     864             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     865             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     866             :     */
+     867           2 :   bool loadParam(const std::string& name, XmlRpc::XmlRpcValue& out, const XmlRpc::XmlRpcValue& default_value)
+     868             :   {
+     869           2 :     return loadParam<XmlRpc::XmlRpcValue>(name, out, default_value);
+     870             :   }
+     871             : 
+     872             :   /*!
+     873             :     * \brief An overload for loading a compulsory XmlRpc::XmlRpcValue.
+     874             :     *
+     875             :     * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+     876             :     * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+     877             :     * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+     878             :     *
+     879             :     * \param name          Name of the parameter in the rosparam server.
+     880             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     881             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     882             :     */
+     883           3 :   bool loadParam(const std::string& name, XmlRpc::XmlRpcValue& out)
+     884             :   {
+     885           3 :     return loadParam<XmlRpc::XmlRpcValue>(name, out);
+     886             :   }
+     887             : 
+     888             :   /*!
+     889             :     * \brief An overload for loading an optional XmlRpc::XmlRpcValue.
+     890             :     *
+     891             :     * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+     892             :     * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+     893             :     * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+     894             :     *
+     895             :     * \param name          Name of the parameter in the rosparam server.
+     896             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     897             :     * \return              the loaded parameter.
+     898             :     */
+     899           1 :   XmlRpc::XmlRpcValue loadParam2(const std::string& name, const XmlRpc::XmlRpcValue& default_value)
+     900             :   {
+     901           1 :     return loadParam2<XmlRpc::XmlRpcValue>(name, default_value);
+     902             :   }
+     903             : 
+     904             :   //}
+     905             : 
+     906             :   /* loadParam specializations and convenience functions for Eigen dynamic matrix type //{ */
+     907             : 
+     908             :   /*!
+     909             :     * \brief An overload for loading Eigen matrices.
+     910             :     *
+     911             :     * Matrix dimensions are deduced from the provided default value.
+     912             :     * For compulsory Eigen matrices, use loadMatrixStatic(), loadMatrixKnown() or loadMatrixDynamic().
+     913             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     914             :     * the default value is used.
+     915             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     916             :     *
+     917             :     * \param name          Name of the parameter in the rosparam server.
+     918             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     919             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     920             :     * \return              True if loaded successfully, false otherwise.
+     921             :     */
+     922             :   template <typename T>
+     923             :   bool loadParam(const std::string& name, MatrixX<T>& mat, const MatrixX<T>& default_value)
+     924             :   {
+     925             :     const int rows = default_value.rows();
+     926             :     const int cols = default_value.cols();
+     927             :     const bool loaded_ok = loadMatrixDynamic(name, mat, default_value, rows, cols);
+     928             :     return loaded_ok;
+     929             :   }
+     930             : 
+     931             :   /*!
+     932             :     * \brief An overload for loading Eigen matrices.
+     933             :     *
+     934             :     * Matrix dimensions are deduced from the provided default value.
+     935             :     * For compulsory Eigen matrices, use loadMatrixStatic(), loadMatrixKnown() or loadMatrixDynamic().
+     936             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     937             :     * the default value is used.
+     938             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     939             :     *
+     940             :     * \param name          Name of the parameter in the rosparam server.
+     941             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     942             :     * \return              The loaded parameter value.
+     943             :     */
+     944             :   template <typename T>
+     945             :   MatrixX<T> loadParam2(const std::string& name, const MatrixX<T>& default_value)
+     946             :   {
+     947             :     MatrixX<T> ret;
+     948             :     loadParam(name, ret, default_value);
+     949             :     return ret;
+     950             :   }
+     951             :   
+     952             :   //}
+     953             : 
+     954             :   // loadMatrixStatic function for loading of static Eigen::Matrices //{
+     955             : 
+     956             :   /*!
+     957             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+     958             :     *
+     959             :     * This variant assumes that the matrix dimensions are known in compiletime.
+     960             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+     961             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     962             :     * the loading process is unsuccessful.
+     963             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     964             :     *
+     965             :     * \tparam rows  Expected number of rows of the matrix.
+     966             :     * \tparam cols  Expected number of columns of the matrix.
+     967             :     *
+     968             :     * \param name  Name of the parameter in the rosparam server.
+     969             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     970             :     * \return      true if loaded successfully, false otherwise.
+     971             :     *
+     972             :     */
+     973             :   template <int rows, int cols, typename T>
+     974           4 :   bool loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat)
+     975             :   {
+     976           4 :     const auto [ret, loaded_ok] = loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>::Zero(), COMPULSORY, UNIQUE);
+     977           4 :     mat = ret;
+     978           4 :     return loaded_ok;
+     979             :   }
+     980             : 
+     981             :   /*!
+     982             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+     983             :     *
+     984             :     * This variant assumes that the matrix dimensions are known in compiletime.
+     985             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+     986             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+     987             :     * the default value is used.
+     988             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     989             :     *
+     990             :     * \tparam rows          Expected number of rows of the matrix.
+     991             :     * \tparam cols          Expected number of columns of the matrix.
+     992             :     *
+     993             :     * \param name          Name of the parameter in the rosparam server.
+     994             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     995             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     996             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     997             :     *
+     998             :     */
+     999             :   template <int rows, int cols, typename T, typename Derived>
+    1000          97 :   bool loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat, const Eigen::MatrixBase<Derived>& default_value)
+    1001             :   {
+    1002          97 :     const auto [ret, loaded_ok] = loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>(default_value), OPTIONAL, UNIQUE);
+    1003          97 :     mat = ret;
+    1004          97 :     return loaded_ok;
+    1005             :   }
+    1006             : 
+    1007             :   /*!
+    1008             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+    1009             :     *
+    1010             :     * This variant assumes that the matrix dimensions are known in compiletime.
+    1011             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1012             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1013             :     * the loading process is unsuccessful.
+    1014             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1015             :     *
+    1016             :     * \tparam rows  Expected number of rows of the matrix.
+    1017             :     * \tparam cols  Expected number of columns of the matrix.
+    1018             :     *
+    1019             :     * \param name  Name of the parameter in the rosparam server.
+    1020             :     * \return      The loaded parameter value.
+    1021             :     *
+    1022             :     */
+    1023             :   template <int rows, int cols, typename T = double>
+    1024           2 :   Eigen::Matrix<T, rows, cols> loadMatrixStatic2(const std::string& name)
+    1025             :   {
+    1026           2 :     Eigen::Matrix<T, rows, cols> ret;
+    1027           2 :     loadMatrixStatic(name, ret);
+    1028           2 :     return ret;
+    1029             :   }
+    1030             : 
+    1031             :   /*!
+    1032             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1033             :     *
+    1034             :     * This variant assumes that the matrix dimensions are known in compiletime.
+    1035             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1036             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1037             :     * the default value is used.
+    1038             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1039             :     *
+    1040             :     * \tparam rows          Expected number of rows of the matrix.
+    1041             :     * \tparam cols          Expected number of columns of the matrix.
+    1042             :     *
+    1043             :     * \param name          Name of the parameter in the rosparam server.
+    1044             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1045             :     * \return              The loaded parameter value.
+    1046             :     *
+    1047             :     */
+    1048             :   template <int rows, int cols, typename T, typename Derived>
+    1049           1 :   Eigen::Matrix<T, rows, cols> loadMatrixStatic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value)
+    1050             :   {
+    1051           1 :     Eigen::Matrix<T, rows, cols> ret;
+    1052           1 :     loadMatrixStatic(name, ret, default_value);
+    1053           1 :     return ret;
+    1054             :   }
+    1055             :   //}
+    1056             : 
+    1057             :   // loadMatrixKnown function for loading of Eigen matrices with known dimensions //{
+    1058             : 
+    1059             :   /*!
+    1060             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+    1061             :     *
+    1062             :     * This variant assumes that the matrix dimensions are known in runtime, but not compiletime.
+    1063             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1064             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1065             :     * the loading process is unsuccessful.
+    1066             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1067             :     *
+    1068             :     * \param name  Name of the parameter in the rosparam server.
+    1069             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1070             :     * \param rows  Expected number of rows of the matrix.
+    1071             :     * \param cols  Expected number of columns of the matrix.
+    1072             :     * \return      true if loaded successfully, false otherwise.
+    1073             :     */
+    1074             :   template <typename T>
+    1075         378 :   bool loadMatrixKnown(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1076             :   {
+    1077         756 :     const auto [ret, loaded_ok] = loadMatrixKnown_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1078         378 :     mat = ret;
+    1079         756 :     return loaded_ok;
+    1080             :   }
+    1081             : 
+    1082             :   /*!
+    1083             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1084             :     *
+    1085             :     * This variant assumes that the matrix dimensions are known in runtime, but not compiletime.
+    1086             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1087             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1088             :     * the default value is used.
+    1089             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1090             :     *
+    1091             :     * \param name          Name of the parameter in the rosparam server.
+    1092             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1093             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1094             :     * \param rows          Expected number of rows of the matrix.
+    1095             :     * \param cols          Expected number of columns of the matrix.
+    1096             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+    1097             :     */
+    1098             :   template <typename T, typename Derived>
+    1099           2 :   bool loadMatrixKnown(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1100             :   {
+    1101           4 :     const auto [ret, loaded_ok] = loadMatrixKnown_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1102           2 :     mat = ret;
+    1103           4 :     return loaded_ok;
+    1104             :   }
+    1105             : 
+    1106             :   /*!
+    1107             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+    1108             :     *
+    1109             :     * This variant assumes that the matrix dimensions are known in runtime, but not compiletime.
+    1110             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1111             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1112             :     * the loading process is unsuccessful.
+    1113             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1114             :     *
+    1115             :     * \param name  Name of the parameter in the rosparam server.
+    1116             :     * \param rows  Expected number of rows of the matrix.
+    1117             :     * \param cols  Expected number of columns of the matrix.
+    1118             :     * \return      The loaded parameter value.
+    1119             :     */
+    1120             :   template <typename T = double>
+    1121           1 :   MatrixX<T> loadMatrixKnown2(const std::string& name, int rows, int cols)
+    1122             :   {
+    1123           1 :     MatrixX<T> ret;
+    1124           1 :     loadMatrixKnown(name, ret, rows, cols);
+    1125           1 :     return ret;
+    1126             :   }
+    1127             : 
+    1128             :   /*!
+    1129             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1130             :     *
+    1131             :     * This variant assumes that the matrix dimensions are known in runtime, but not compiletime.
+    1132             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1133             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1134             :     * the default value is used.
+    1135             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1136             :     *
+    1137             :     * \param name          Name of the parameter in the rosparam server.
+    1138             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1139             :     * \param rows          Expected number of rows of the matrix.
+    1140             :     * \param cols          Expected number of columns of the matrix.
+    1141             :     * \return              The loaded parameter value.
+    1142             :     */
+    1143             :   template <typename T, typename Derived>
+    1144           1 :   MatrixX<T> loadMatrixKnown2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1145             :   {
+    1146           1 :     MatrixX<T> ret;
+    1147           1 :     loadMatrixKnown(name, ret, default_value, rows, cols);
+    1148           1 :     return ret;
+    1149             :   }
+    1150             :   //}
+    1151             : 
+    1152             :   // loadMatrixDynamic function for half-dynamic loading of MatrixX<T> //{
+    1153             : 
+    1154             :   /*!
+    1155             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1156             :     *
+    1157             :     * This variant assumes that the only one of the matrix dimensions are known, the other is selected based on the loaded value.
+    1158             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1159             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1160             :     * the loading process is unsuccessful.
+    1161             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1162             :     *
+    1163             :     * \param name  Name of the parameter in the rosparam server.
+    1164             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1165             :     * \param rows  Expected number of rows of the matrix (negative value indicates that the number of rows is to be deduced from the specified number of columns and the size of the loaded array).
+    1166             :     * \param cols  Expected number of columns of the matrix (negative value indicates that the number of columns is to be deduced from the specified number of rows and the size of the loaded array).
+    1167             :     * \return      true if loaded successfully, false otherwise.
+    1168             :     */
+    1169             :   template <typename T>
+    1170          76 :   bool loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1171             :   {
+    1172         152 :     const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1173          76 :     mat = ret;
+    1174         152 :     return loaded_ok;
+    1175             :   }
+    1176             : 
+    1177             :   /*!
+    1178             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1179             :     *
+    1180             :     * This variant assumes that the only one of the matrix dimensions are known, the other is selected based on the loaded value.
+    1181             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1182             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1183             :     * the default value is used.
+    1184             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1185             :     *
+    1186             :     * \param name          Name of the parameter in the rosparam server.
+    1187             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1188             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1189             :     * \param rows          Expected number of rows of the matrix (negative value indicates that the number of rows is to be deduced from the specified number of columns and the size of the loaded array).
+    1190             :     * \param cols          Expected number of columns of the matrix (negative value indicates that the number of columns is to be deduced from the specified number of rows and the size of the loaded array).
+    1191             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+    1192             :     */
+    1193             :   template <typename T, typename Derived>
+    1194          96 :   bool loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1195             :   {
+    1196         192 :     const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1197          96 :     mat = ret;
+    1198         192 :     return loaded_ok;
+    1199             :   }
+    1200             : 
+    1201             :   /*!
+    1202             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1203             :     *
+    1204             :     * This variant assumes that the only one of the matrix dimensions are known, the other is selected based on the loaded value.
+    1205             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1206             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1207             :     * the loading process is unsuccessful.
+    1208             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1209             :     *
+    1210             :     * \param name  Name of the parameter in the rosparam server.
+    1211             :     * \param rows  Expected number of rows of the matrix (negative value indicates that the number of rows is to be deduced from the specified number of columns and the size of the loaded array).
+    1212             :     * \param cols  Expected number of columns of the matrix (negative value indicates that the number of columns is to be deduced from the specified number of rows and the size of the loaded array).
+    1213             :     * \return      The loaded parameter value.
+    1214             :     */
+    1215             :   template <typename T = double>
+    1216          74 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, int rows, int cols)
+    1217             :   {
+    1218          74 :     MatrixX<T> ret;
+    1219          74 :     loadMatrixDynamic(name, ret, rows, cols);
+    1220          74 :     return ret;
+    1221             :   }
+    1222             : 
+    1223             :   /*!
+    1224             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1225             :     *
+    1226             :     * This variant assumes that the only one of the matrix dimensions are known, the other is selected based on the loaded value.
+    1227             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1228             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1229             :     * the default value is used.
+    1230             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1231             :     *
+    1232             :     * \param name          Name of the parameter in the rosparam server.
+    1233             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1234             :     * \param rows          Expected number of rows of the matrix (negative value indicates that the number of rows is to be deduced from the specified number of columns and the size of the loaded array).
+    1235             :     * \param cols          Expected number of columns of the matrix (negative value indicates that the number of columns is to be deduced from the specified number of rows and the size of the loaded array).
+    1236             :     * \return              The loaded parameter value.
+    1237             :     */
+    1238             :   template <typename T, typename Derived>
+    1239           1 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1240             :   {
+    1241           1 :     MatrixX<T> ret;
+    1242           1 :     loadMatrixDynamic(name, ret, default_value, rows, cols);
+    1243           1 :     return ret;
+    1244             :   }
+    1245             : 
+    1246             :   //}
+    1247             : 
+    1248             :   // loadMatrixArray function for loading of an array of MatrixX<T> with known dimensions //{
+    1249             :   /*!
+    1250             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1251             :     *
+    1252             :     * The number of rows and columns of the matrices to be loaded is specified in the \c rosparam parameter. Specifically, the \c name/rows value specifies the
+    1253             :     * number of rows, which must be common to all the loaded matrices (i.e. it is one integer >= 0), and the \c name/cols value specifies the number of columns of
+    1254             :     * each matrix (i.e. it is an array of integers > 0). The \c name/data array contains the values of the elements of the matrices and it must have length
+    1255             :     * \f$ r\sum_i c_i \f$, where \f$ r \f$ is the common number of rows and \f$ c_i \f$ is the number of columns of the \f$ i \f$-th matrix.
+    1256             :     * A typical structure of a \c yaml file, specifying the
+    1257             :     * matrix array to be loaded using this method, is
+    1258             :     *
+    1259             :     * \code{.yaml}
+    1260             :     *
+    1261             :     * matrix_array:
+    1262             :     *   rows: 3
+    1263             :     *   cols: [1, 2]
+    1264             :     *   data: [-5.0, 0.0, 23.0,
+    1265             :     *          -5.0, 0.0, 12.0,
+    1266             :     *           2.0,   4.0,  7.0]
+    1267             :     *
+    1268             :     * \endcode
+    1269             :     *
+    1270             :     * which will be loaded as a \c std::vector, containing one \f$ 3\times 1 \f$ matrix and one \f$ 3\times 2 \f$ matrix.
+    1271             :     *
+    1272             :     * If the dimensions of the loaded matrices do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1273             :     * If the parameter with the specified name is not found on the \c rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1274             :     * the loading process is unsuccessful.
+    1275             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1276             :     *
+    1277             :     * \param name  Name of the parameter in the rosparam server.
+    1278             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1279             :     *
+    1280             :     */
+    1281             :   template <typename T>
+    1282           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat)
+    1283             :   {
+    1284           1 :     mat = loadMatrixArray2<double>(name);
+    1285           1 :   }
+    1286             : 
+    1287             :   /*!
+    1288             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1289             :     *
+    1290             :     * This overload of the loadMatrixArray() method takes a default value for the parameter, which is used in case a \c rosparam with the specified name is not
+    1291             :     * found in the \c rosparam server, instead of causing an unsuccessful load. This makes specifying the parameter value in the \c rosparam server optional.
+    1292             :     *
+    1293             :     * \param name           Name of the parameter in the rosparam server.
+    1294             :     * \param mat            Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1295             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1296             :     *
+    1297             :     */
+    1298             :   template <typename T>
+    1299           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat, const std::vector<MatrixX<T>>& default_value)
+    1300             :   {
+    1301           1 :     mat = loadMatrixArray2(name, default_value);
+    1302           1 :   }
+    1303             : 
+    1304             :   /*!
+    1305             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1306             :     *
+    1307             :     * This method works in the same way as the loadMatrixArray() method for compulsory parameters, except that the loaded
+    1308             :     * parameter is returned and not stored in the reference parameter.
+    1309             :     *
+    1310             :     * \param name           Name of the parameter in the rosparam server.
+    1311             :     * \returns              The loaded parameter or a default constructed object of the respective type.
+    1312             :     *
+    1313             :     */
+    1314             :   template <typename T = double>
+    1315           3 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name)
+    1316             :   {
+    1317           3 :     return loadMatrixArray_internal(name, std::vector<MatrixX<T>>(), COMPULSORY, UNIQUE);
+    1318             :   }
+    1319             : 
+    1320             :   /*!
+    1321             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1322             :     *
+    1323             :     * This method works in the same way as the loadMatrixArray() method for optional parameters, except that the loaded
+    1324             :     * parameter is returned and not stored in the reference parameter.
+    1325             :     *
+    1326             :     * \param name           Name of the parameter in the rosparam server.
+    1327             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1328             :     * \returns              The loaded parameter or the default value.
+    1329             :     *
+    1330             :     */
+    1331             :   template <typename T>
+    1332           2 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name, const std::vector<MatrixX<T>>& default_value)
+    1333             :   {
+    1334           2 :     return loadMatrixArray_internal(name, default_value, OPTIONAL, UNIQUE);
+    1335             :   }
+    1336             :   //}
+    1337             : 
+    1338             :   //}
+    1339             : 
+    1340             : };
+    1341             : //}
+    1342             : 
+    1343             :   /*!
+    1344             :     * \brief An overload for loading ros::Duration.
+    1345             :     *
+    1346             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1347             :     *
+    1348             :     * \param name          Name of the parameter in the rosparam server.
+    1349             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1350             :     * \return              The loaded parameter value.
+    1351             :     */
+    1352             :   template <>
+    1353             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name, const ros::Duration& default_value);
+    1354             : 
+    1355             :   /*!
+    1356             :     * \brief An overload for loading ros::Duration.
+    1357             :     *
+    1358             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1359             :     *
+    1360             :     * \param name          Name of the parameter in the rosparam server.
+    1361             :     * \return              The loaded parameter value.
+    1362             :     */
+    1363             :   template <>
+    1364             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name);
+    1365             : 
+    1366             : }  // namespace mrs_lib
+    1367             : 
+    1368             : #endif  // PARAM_LOADER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html b/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html new file mode 100644 index 0000000000..06a945b67d --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html @@ -0,0 +1,362 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/param_loader.h.gcov.png b/mrs_lib/include/mrs_lib/param_loader.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a4d4e2202f27269e5f25a66abde48f1cb954de81 GIT binary patch literal 4406 zcmV-65y|d}P)?dmV;-Eh=eLQB(e%tMAqI2=3>TAChE z6XB?B!BO9Imv2F+82;!2@QoQAj zkZ*hw+~lD>kBvuS_ys)H7n4V__?8Tj14BX9V)A&?$??GD*tSgA3lD6N#b*AEqjsTa z*s(Q>FI^&ZLG;^$kUUBuzJO5&9u0r8XjMUvArLy4CJh32(&l-_<3&-IavBKMTUI zudn~;`u>T(e*XLZ?|Xgy^F^0oFn2MS8O2m1JXbs&?)${EiUp+f6JLMr6OWG4itvP2 zH$=IPqtfivR3ZMqrv*=R(mm$#qP)jOKAATu>%Uhv`2;r54`wgIVP>gTZTeHG(3soVzSj z(~53d24hz)@q63Kk;;|&bgptiEYOSqI9#Jxg%Un5B4Asl4<3TrBg-2i4Kbe z?N3BPugQ?8L=gksHlZy$!y3n0`gE=Mm9q0N>MFAwHLdHWJ{7l2VaV$m^}guiUJ)@v zp@ND+PE=&i$?YHaTr*jh(>#}Sk_W;8t06yBE< zcFoH|GF@tfx+o>GBy|`PF4*Kuan(JBR`-HXDi|RsP41pPcIQM8YUQ|{nFh>kGc)(Y zM=^!tU|83-eG1E&k?OsQf#+J)^b@C0VN0!3IL>`e7>qgukVdNCwAM7MuU4oa?irhw z=49C#i;U3hhP%q}c&Q2AQ#tOG4C&~&Fn8=i=L=0YI`+bl)(O>kJbcty27o1JCv#7i z(wB`-V4Uw4k3u;1%1746nL{R0qeT+-+zdrhLw#8Ls8X8AV9lWE!Ql1cF{Fj3zzmW* zTw?ZI0QNay2t>{+&9uS}#D*+RKz0$Y0lr|V_nBtKPz?CCgfNFxEoyQUD-g{|2c3bM zal-aOFrT>rRIY6>Q}h9B_;<9}Y|Otl*R;J=ztSASlw^!E`^%u%+gUGmgAgF z?_w9L0mT3jkfiM6y^|I8g46ewA&A3c$Q;Sm2O#)NLlqru0_)mZyXF_ zODFE3T-Q4s<4o)mbhK5{Q+Sv9$o6b*8J84%phx&&drzq*+ybxgmE1|Z+4Qby(92R7 zi_Ja&DV%{JRZn5sF6(k3IhayRzX&}eP+HNWh>(g=-@)d}QdopYPS{DNEPXs-$%n)n zSLE92s;;HjBB^AHmNwswz?TN)Hpy-9JS`<1&9u#PiM?)UCmJ(Di(freyi6^nlEPxb zi@7W6oo{Bb`ApAuO8yGfH0Q1}@RO!5<0^_*eG7CQfurZkDhwgnvj>X>JOS_r^Xi_e z20*i$c7;(;Un7Ljtg|CWuZ7CoYiYt{8Uld}8Ks9pG+}do*wvxu*bE zE60g;C&2?VFtSp%g$P1C7+ep;v!lX3-2Zs%6+Ct|{XD(?WzWUA(k=tg z@mpt=xrbX#cnYycBf`1XR+VE);flqi8G}VLq3)V)rNrB*eLfoV2JS6EjVIdIK|XFO_^KVv!LF-Vl`3*bGNyO36to1nkmZG2Nu4IDNzDpp=sF~PZc19oLo3_Kd38UP zR2@~b(n$yK+4TGamXuLHJw9(dmoUQ%ycD3NQe6mrr=TvmJ?A-*~6XEvq397sQl<(Jt>FvLe12^?`Btv|W{gx^)SPY~8gl z$ItTB0oU=XQj@e5XmZ8LH&}qkmalreZOd14nhmr7Vpi7jp?`nLkN$P3Zh$Cwiz6pcpegr^H=7jn5j zxs;6&;mB3&oWZdHw9Z8Mslb)DSl|oamwzf}<|RP!7w0-V4uDmHDl}s8&I`)kz>G%` z?2xQJesrod^mM7Iad{P@4FD%i1nZ!uTGHdMk=ciJYrJrvNF^R#GhU8Wv>ds!DEe>ZI45D z#Zd)930T=fE+0gN2rC&_<(&vUQ?W&z&(-y5{1th&Gvo(j*gZ)O~Yk(q1Q>DWi-&e&(BH`AAx8D7Xm1GG_` zeg2wVrDNyqKzB?h+UMNJaf@#knD9hu`jNsx`s7@`UalgtyV11GnouKETa@%XNX^$&Gz% zc%QHgn_2qem91`urXx^2-pdBCsuqAiGaAPdYF{T5_fZuH16(e^cg9nb-%}MXYr|=+ zsRTG;++IL3A>1AMg+FJnD^TaRIlxZ$(bAVlsOMKx9>2Nf>kcI2v~Xa=UrDBMJ2!J@!6=%+cCQ2=LsI+p$~m zZHg{D&!`6>?@1bfbq|t?FxkVRnj_x=VfWkB%chT1F7@?6_^(GPJ>bf*Xb1f;aJMbJ z6>!xY>+U1$_aF}|*H*u5%7qZ+uR|x`dz{;k`^dFWbjIpF0`eNRt^4Y2?=wYoqLcnA zggVOuEOkce`X=Q))zzehMq%;f$YH z)IW%*y&eFrFvywV_yBhZ+~38c6+LY{E(PJG@c1#L$AvRB?CJ5yfZ5`bZa$31!a6=1 zk7|z9;De0-0PU1MRswccoo(wP3F6!%XLu6?A%b6wm4tvVrr+AEM0th7xAswABb-4O z>aPsn=r&r115^e`wyjXtyQP*El*IiCC6ChkC zXrY(^FEA|~$&O-yCb{QU|6H*Eg)L%DwuYzmk(+TsWogF2{7%*w7yn^wiuQmu8%#p4 z#R(gz)`a$b27uaW2<{vM;2~spseSC*kdV|rYt3qV`gVs2dtrhPbkdg}d4E$MZAQuy zT3Y5R-&&L%%qcU}dGFSu@;k>lv53b9Yfjt~NySl*U+S+%4S>*EMA1tbfagR29@^|0 zfd=8$2%HssOo^~*1g5=FO0U%PJC*{+D_Z+lih&$Ip~5FrsD=uDlzEt%enhWpUs@@@ zOJ~lDciD;ldk!~1md)?-%^T};Y_Z7*ucz0a2;hAqfFP9Y(elGH9sAe-m``fhV*u!k z*E->`2H?cd6e&f)28aW=~ui0l$4_OL_ z@MepV^!o4m>xa^2=rP&wEuj=Cu$(TFHn;QUY+CyCC5N1)i4zd*Vb28}+fjT&;Eue{xI0g(VDY}cao(8y5 zPiT8Q4Av1aU9v$yITSU6u9sQ6)(vdyzZPf>P1+eLkoEVx^ithO7eJ5fM@F5Ij zH1@W^pkd*VjABfb3*Zq5C5aQ-k%=cvA~?mMPq;X3AD;wVTWppAw>$^ny@5+fiLXG| zQ@|c*0a4^V*ahNf8;laT$X?_T9OaPv&^CQeYM*5Z1v2gEDUy$ZGyd_yiTGRNQD+|; zz$HPM`*?yO9RT-;gQsd=4))xY+@m?F*L{y4%b!1I7}Kt-)P#w{bXEmPGoV1usd8n} wp7#2~bA9D0G;I6uRMI)h6s*m+r07*qoM6N<$f)oynZvX%Q literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html b/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html new file mode 100644 index 0000000000..7dd6c6848e --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html @@ -0,0 +1,556 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - publisher_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:11811999.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()0
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler()1
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler_impl()1
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler()2
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler()4
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()31
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()62
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()95
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()99
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()189
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()194
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()194
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()194
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()271
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()284
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()293
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()370
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()370
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()376
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()376
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()376
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()376
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()388
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()459
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()543
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()543
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()543
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()564
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()564
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()577
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()643
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()708
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()708
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()740
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()752
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()752
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()914
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()991
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()1128
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1416
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1534
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()140527
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/publisher_handler.h.func.html b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html new file mode 100644 index 0000000000..3e696374c5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html @@ -0,0 +1,556 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - publisher_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:11811999.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()376
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()752
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()194
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()293
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()564
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()1128
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()991
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1534
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()370
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()740
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler()2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler()4
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler()1
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()708
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1416
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()643
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()914
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()189
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()284
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()543
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()543
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()282
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()194
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()388
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()577
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()140527
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()62
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()94
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()188
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()376
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()752
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()376
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()99
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()564
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()543
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()370
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler_impl()1
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()708
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()271
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()95
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()0
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()194
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()459
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()31
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::~PublisherHandler_impl()94
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()376
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.frameset.html new file mode 100644 index 0000000000..79755e1cb1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html new file mode 100644 index 0000000000..0d84edcf56 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - publisher_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:11811999.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Defines PublisherHandler and related convenience classes for upgrading the ROS publisher
+       3             :      \author Tomas Baca - tomas.baca@fel.cvut.cz
+       4             :  */
+       5             : #ifndef PUBLISHER_HANDLER_H
+       6             : #define PUBLISHER_HANDLER_H
+       7             : 
+       8             : #include <ros/ros.h>
+       9             : #include <ros/package.h>
+      10             : 
+      11             : #include <atomic>
+      12             : #include <string>
+      13             : #include <mutex>
+      14             : 
+      15             : namespace mrs_lib
+      16             : {
+      17             : 
+      18             : /* class PublisherHandler_impl //{ */
+      19             : 
+      20             : /**
+      21             :  * @brief implementation of the publisher handler
+      22             :  */
+      23             : template <class TopicType>
+      24             : class PublisherHandler_impl {
+      25             : 
+      26             : public:
+      27             :   /**
+      28             :    * @brief default constructor
+      29             :    */
+      30             :   PublisherHandler_impl(void);
+      31             : 
+      32             :   /**
+      33             :    * @brief default destructor
+      34             :    */
+      35        6347 :   ~PublisherHandler_impl(void){};
+      36             : 
+      37             :   /**
+      38             :    * @brief constructor
+      39             :    *
+      40             :    * @param nh ROS node handler
+      41             :    * @param address topic address
+      42             :    * @param buffer_size buffer size
+      43             :    * @param latch latching
+      44             :    */
+      45             :   PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size = 1, const bool& latch = false,
+      46             :                         const double& rate = 0.0);
+      47             : 
+      48             :   /**
+      49             :    * @brief publish message
+      50             :    *
+      51             :    * @param msg data
+      52             :    *
+      53             :    */
+      54             :   void publish(const TopicType& msg);
+      55             : 
+      56             :   /**
+      57             :    * @brief publish message, boost ptr overload
+      58             :    *
+      59             :    * @param msg
+      60             :    */
+      61             :   void publish(const boost::shared_ptr<TopicType>& msg);
+      62             : 
+      63             :   /**
+      64             :    * @brief publish message, boost const ptr overload
+      65             :    *
+      66             :    * @param msg
+      67             :    */
+      68             :   void publish(const boost::shared_ptr<TopicType const>& msg);
+      69             : 
+      70             :   /**
+      71             :    * @brief get number of subscribers
+      72             :    *
+      73             :    * @return the number of subscribers
+      74             :    */
+      75             :   unsigned int getNumSubscribers(void);
+      76             : 
+      77             : private:
+      78             :   ros::Publisher    publisher_;
+      79             :   std::mutex        mutex_publisher_;
+      80             :   std::atomic<bool> publisher_initialized_;
+      81             : 
+      82             :   bool      throttle_ = false;
+      83             :   double    throttle_min_dt_;
+      84             :   ros::Time last_time_published_;
+      85             : };
+      86             : 
+      87             : //}
+      88             : 
+      89             : /* class PublisherHandler //{ */
+      90             : 
+      91             : /**
+      92             :  * @brief user wrapper of the publisher handler implementation
+      93             :  */
+      94             : template <class TopicType>
+      95             : class PublisherHandler {
+      96             : 
+      97             : public:
+      98             :   /**
+      99             :    * @brief generic constructor
+     100             :    */
+     101        8767 :   PublisherHandler(void){};
+     102             : 
+     103             :   /**
+     104             :    * @brief generic destructor
+     105             :    */
+     106      154605 :   ~PublisherHandler(void){};
+     107             : 
+     108             :   /**
+     109             :    * @brief operator=
+     110             :    *
+     111             :    * @param other
+     112             :    *
+     113             :    * @return
+     114             :    */
+     115             :   PublisherHandler& operator=(const PublisherHandler& other);
+     116             : 
+     117             :   /**
+     118             :    * @brief copy constructor
+     119             :    *
+     120             :    * @param other
+     121             :    */
+     122             :   PublisherHandler(const PublisherHandler& other);
+     123             : 
+     124             :   /**
+     125             :    * @brief constructor
+     126             :    *
+     127             :    * @param nh ROS node handler
+     128             :    * @param address topic address
+     129             :    * @param buffer_size buffer size
+     130             :    * @param latch latching
+     131             :    */
+     132         226 :   PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size = 1, const bool& latch = false, const double& rate = 0);
+     133             : 
+     134             :   /**
+     135             :    * @brief publish message
+     136             :    *
+     137             :    * @param msg
+     138             :    */
+     139             :   void publish(const TopicType& msg);
+     140             : 
+     141             :   /**
+     142             :    * @brief publish message, boost ptr overload
+     143             :    *
+     144             :    * @param msg
+     145             :    */
+     146             :   void publish(const boost::shared_ptr<TopicType>& msg);
+     147             : 
+     148             :   /**
+     149             :    * @brief publish message, boost const ptr overload
+     150             :    *
+     151             :    * @param msg
+     152             :    */
+     153             :   void publish(const boost::shared_ptr<TopicType const>& msg);
+     154             : 
+     155             :   /**
+     156             :    * @brief get number of subscribers
+     157             :    *
+     158             :    * @return the number of subscribers
+     159             :    */
+     160             :   unsigned int getNumSubscribers(void);
+     161             : 
+     162             : private:
+     163             :   std::shared_ptr<PublisherHandler_impl<TopicType>> impl_;
+     164             : };
+     165             : 
+     166             : //}
+     167             : 
+     168             : }  // namespace mrs_lib
+     169             : 
+     170             : #include <mrs_lib/impl/publisher_handler.hpp>
+     171             : 
+     172             : #endif  // PUBLISHER_HANDLER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.overview.html b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.overview.html new file mode 100644 index 0000000000..f7a333c6d4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.overview.html @@ -0,0 +1,63 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.png b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2e1368f216e722628217c675653cec3b3eb2b964 GIT binary patch literal 572 zcmV-C0>k}@P)jIAVzQI3tdiO%^l}?ut?wSfv!tU=`04H9tJcLlr+lB{7K683_n#T%WJ{My@|tq@(patK5dhlgse{rQX(#CEF!X)&kewkVJscYV zsh+Cni4?QkaVndNnwyi^g`d6UpdOhmj*hlizMtk!up88{?APwdz~*6pUFgIHJZqfZ zU)Sxm5A+Edm8FN=p}Q_YRe)Ev*KpzvO@DoRy&~=C#U1HI?e%Az87@#Bn!pBh=L`hY zcFlZv#r_#+YBZDz*{KT%Hw+b2U!Xf`XuQWgagWA9{Q1~!{_f>3ybtfLE_+j_H~@GL zXCm+(&P3onoJohx4-IE;avXzxVN7A_|NCx}Mn<1h?rE)5c5(kNrF{`P`xrm~0000< KMNUMnLSTX}3jTlq literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html new file mode 100644 index 0000000000..2ab70256b9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::quadratic_throttle_model::forceToThrottle(mrs_lib::quadratic_throttle_model::MotorParams_t, double)83268
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)119845
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html new file mode 100644 index 0000000000..4d98316165 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::quadratic_throttle_model::forceToThrottle(mrs_lib::quadratic_throttle_model::MotorParams_t, double)83268
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)119845
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.frameset.html new file mode 100644 index 0000000000..aa416cc5c8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html new file mode 100644 index 0000000000..3f45c11c73 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html @@ -0,0 +1,117 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef QUADRATIC_THRUST_MODEL_H
+       2             : #define QUADRATIC_THRUST_MODEL_H
+       3             : 
+       4             : #include <cmath>
+       5             : 
+       6             : namespace mrs_lib
+       7             : {
+       8             : 
+       9             : namespace quadratic_throttle_model
+      10             : {
+      11             : 
+      12             : typedef struct
+      13             : {
+      14             :   double A;
+      15             :   double B;
+      16             :   int    n_motors;
+      17             : } MotorParams_t;
+      18             : 
+      19      119845 : double inline throttleToForce(const MotorParams_t motor_params, const double throttle) {
+      20             : 
+      21      119845 :   return motor_params.n_motors * pow((throttle - motor_params.B) / motor_params.A, 2);
+      22             : }
+      23             : 
+      24       83268 : double inline forceToThrottle(const MotorParams_t motor_params, const double force) {
+      25             : 
+      26       83268 :   return sqrt(force / motor_params.n_motors) * motor_params.A + motor_params.B;
+      27             : }
+      28             : 
+      29             : }  // namespace quadratic_throttle_model
+      30             : 
+      31             : }  // namespace mrs_lib
+      32             : 
+      33             : #endif  // QUADRATIC_THRUST_MODEL_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.overview.html b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.overview.html new file mode 100644 index 0000000000..9573c3e7be --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.overview.html @@ -0,0 +1,29 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.png b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..901c7c88f43a90c594501dfbb1127c48124c4bee GIT binary patch literal 246 zcmeAS@N?(olHy`uVBq!ia0vp^0YI$C!VDz$I2!%}QW60^A+G=b|6c_J%iqVwzWUF= zunH&+rp|e9UJ7J$7I;J!GcfQS0b$0e+I-SL!4^*!#}JF&vr}Gk9#G(EdCYc_YnS7u z|9Prn&0vw(Q~TY^(TcNTKb^))zg~3C21Kw6xUb@7~OREMwa1s?Ub`De3_ k`@TQpy=s3*@}p|TV@0+L6F;BJ1iFX8)78&qol`;+04uX!xBvhE literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html b/mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html new file mode 100644 index 0000000000..75e7d56de9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-04-25 05:42:57Functions:204445.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addMeasurement<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::Repredictor(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)1
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t::info_t(ros::Time const&)2
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)2
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addInputChange<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)93
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, double const&)100
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, int const&)100
std::enable_if<!(false), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::predictTo<false>(ros::Time const&)102
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addInputChangeWithNoise<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)212
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t const&)294
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addMeasurement<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, double const&)294
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t> > > > const&)300
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t const&, ros::Time const&, ros::Time const&)588
std::enable_if<true, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::predictTo<true>(ros::Time const&)588
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)631
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)2047
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)5150
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)5716
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)16052
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.func.html b/mrs_lib/include/mrs_lib/repredictor.h.func.html new file mode 100644 index 0000000000..f21d2fe000 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-04-25 05:42:57Functions:204445.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)5150
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)16052
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, double const&)100
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)5716
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)631
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, int const&)100
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t> > > > const&)300
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)2047
std::enable_if<!(false), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::predictTo<false>(ros::Time const&)102
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)1
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t const&)294
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t const&, ros::Time const&, ros::Time const&)588
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addInputChange<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)93
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addMeasurement<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, double const&)294
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addInputChangeWithNoise<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)212
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t::info_t(ros::Time const&)2
std::enable_if<true, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::predictTo<true>(ros::Time const&)588
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)2
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addMeasurement<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::Repredictor(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, unsigned int)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html new file mode 100644 index 0000000000..d18e49adfb --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.html b/mrs_lib/include/mrs_lib/repredictor.h.gcov.html new file mode 100644 index 0000000000..5faff7b9b4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.html @@ -0,0 +1,637 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-04-25 05:42:57Functions:204445.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef REPREDICTOR_H
+       2             : #define REPREDICTOR_H
+       3             : 
+       4             : #include <Eigen/Dense>
+       5             : #include <boost/circular_buffer.hpp>
+       6             : #include <std_msgs/Time.h>
+       7             : #include <functional>
+       8             : #include <ros/ros.h>
+       9             : #include <mrs_lib/utils.h>
+      10             : 
+      11             : namespace mrs_lib
+      12             : {
+      13             :   /**
+      14             :    * \brief Implementation of the Repredictor for fusing measurements with variable delays.
+      15             :    *
+      16             :    * A standard state-space system model is assumed for the repredictor with system inputs and measurements,
+      17             :    * generated from the system state vector. The inputs and measurements may be delayed with varying durations (an older measurement may become available after
+      18             :    * a newer one). A typical use-case scenario is when you have one "fast" but imprecise sensor and one "slow" but precise sensor and you want to use them both
+      19             :    * to get a good prediction (eg. height from the Garmin LiDAR, which comes at 100Hz, and position from a SLAM, which comes at 10Hz with a long delay). If the
+      20             :    * slow sensor is significantly slower than the fast one, fusing its measurement at the time it arrives without taking into account the sensor's delay may
+      21             :    * significantly bias your latest estimate.
+      22             :    *
+      23             :    * To accomodate this, the Repredictor keeps a buffer of N last inputs and measurements (N is specified
+      24             :    * in the constructor). This buffer is then used to re-predict the desired state to a specific time, as
+      25             :    * requested by the user. Note that the re-prediction is evaluated in a lazy manner only when the user requests it,
+      26             :    * so it goes through the whole history buffer every time a prediction is requested.
+      27             :    *
+      28             :    * The Repredictor utilizes a fusion Model (specified as the template parameter), which should implement
+      29             :    * the predict() and correct() methods. This Model is used for fusing the system inputs and measurements
+      30             :    * as well as for predictions. Typically, this Model will be some kind of a Kalman Filter (LKF, UKF etc.).
+      31             :    * \note The Model should be able to accomodate predictions with varying time steps in order for
+      32             :    * the Repredictor to work correctly (see eg. the varstepLKF class).
+      33             :    *
+      34             :    * \tparam Model                  the prediction and correction model (eg. a Kalman Filter).
+      35             :    * \tparam disable_reprediction   if true, reprediction is disabled and the class will act like a dumb LKF (for evaluation purposes).
+      36             :    *
+      37             :    */
+      38             :   template <class Model, bool disable_reprediction=false>
+      39             :   class Repredictor
+      40             :   {
+      41             :   public:
+      42             :     /* states, inputs etc. definitions (typedefs, constants etc) //{ */
+      43             : 
+      44             :     using x_t = typename Model::x_t;                  /*!< \brief State vector type \f$n \times 1\f$ */
+      45             :     using u_t = typename Model::u_t;                  /*!< \brief Input vector type \f$m \times 1\f$ */
+      46             :     using z_t = typename Model::z_t;                  /*!< \brief Measurement vector type \f$p \times 1\f$ */
+      47             :     using P_t = typename Model::P_t;                  /*!< \brief State uncertainty covariance matrix type \f$n \times n\f$ */
+      48             :     using R_t = typename Model::R_t;                  /*!< \brief Measurement noise covariance matrix type \f$p \times p\f$ */
+      49             :     using Q_t = typename Model::Q_t;                  /*!< \brief Process noise covariance matrix type \f$n \times n\f$ */
+      50             :     using statecov_t = typename Model::statecov_t;    /*!< \brief Helper struct for passing around the state and its covariance in one variable */
+      51             :     using ModelPtr = typename std::shared_ptr<Model>; /*!< \brief Shorthand type for a shared pointer-to-Model */
+      52             : 
+      53             :     //}
+      54             : 
+      55             :     /* predictTo() method //{ */
+      56             :     /*!
+      57             :      * \brief Estimates the system state and covariance matrix at the specified time.
+      58             :      *
+      59             :      * The measurement and system input histories are used to estimate the state vector and
+      60             :      * covariance matrix values at the specified time, which are returned.
+      61             :      *
+      62             :      * \param to_stamp   The desired time at which the state vector and covariance matrix should be estimated.
+      63             :      * \return           Returns the estimated state vector and covariance matrix in a single struct.
+      64             :      *
+      65             :      */
+      66             :     template<bool check=disable_reprediction>
+      67         102 :     std::enable_if_t<!check, statecov_t> predictTo(const ros::Time& to_stamp)
+      68             :     {
+      69         102 :       assert(!m_history.empty());
+      70         102 :       auto hist_it = std::begin(m_history);
+      71         102 :       if (hist_it->is_measurement)
+      72             :       {
+      73             :         // if the first history point is measurement, don't use it for correction (to prevent it from being used twice)
+      74           0 :         hist_it->is_measurement = false;
+      75             :       }
+      76             :       // cur_stamp corresponds to the time point of cur_sc estimation
+      77         102 :       auto cur_stamp = hist_it->stamp;
+      78             :       // cur_sc is the current state and covariance estimate
+      79         102 :       auto cur_sc = m_sc;
+      80       15950 :       do
+      81             :       {
+      82       16052 :         cur_sc.stamp = hist_it->stamp;
+      83             :         // do the correction if current history point is a measurement
+      84       16052 :         if ((hist_it->is_measurement))
+      85        5150 :           cur_sc = correctFrom(cur_sc, *hist_it);
+      86             : 
+      87             :         // decide whether to predict to the next history point or straight to the desired stamp already
+      88             :         // (whichever comes sooner or directly to the desired stamp if no further history point is available)
+      89       16052 :         ros::Time next_stamp = to_stamp;
+      90       16052 :         if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
+      91       15950 :           next_stamp = (hist_it + 1)->stamp;
+      92             : 
+      93             :         // do the prediction
+      94       16052 :         cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
+      95       16052 :         cur_stamp = next_stamp;
+      96             : 
+      97             :         // increment the history pointer
+      98       16052 :         hist_it++;
+      99       16052 :       } while (hist_it != std::end(m_history) && hist_it->stamp <= to_stamp);
+     100         102 :       cur_sc.stamp = to_stamp;
+     101         204 :       return cur_sc;
+     102             :     }
+     103             : 
+     104             :     /*!
+     105             :      * \brief Estimates the system state and covariance matrix at the specified time.
+     106             :      *
+     107             :      * The measurement and system input histories are used to estimate the state vector and
+     108             :      * covariance matrix values at the specified time, which are returned.
+     109             :      *
+     110             :      * \param to_stamp   The desired time at which the state vector and covariance matrix should be estimated.
+     111             :      * \return           Returns the estimated state vector and covariance matrix in a single struct.
+     112             :      *
+     113             :      * \note This is the variant of the method when reprediction is disabled and will function like a dumb LKF.
+     114             :      *
+     115             :      */
+     116             :     template<bool check=disable_reprediction>
+     117         588 :     std::enable_if_t<check, statecov_t> predictTo(const ros::Time& to_stamp)
+     118             :     {
+     119         588 :       assert(!m_history.empty());
+     120         588 :       const auto& info = m_history.front();
+     121         588 :       auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
+     122         588 :       sc.stamp = to_stamp;
+     123         588 :       return sc;
+     124             :     }
+     125             :     //}
+     126             : 
+     127             :     /* addInputChangeWithNoise() method //{ */
+     128             :     /*!
+     129             :      * \brief Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
+     130             :      *
+     131             :      * \param u      The system input vector to be added.
+     132             :      * \param Q      The process noise covariance matrix.
+     133             :      * \param stamp  Time stamp of the input vector and covariance matrix.
+     134             :      * \param model  Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default
+     135             :      * model specified in the constructor will be used.
+     136             :      *
+     137             :      * \note The system input vector will not be added if it is older than the oldest element in the history buffer.
+     138             :      *
+     139             :      */
+     140             :     template<bool check=disable_reprediction>
+     141         200 :     std::enable_if_t<!check> addInputChangeWithNoise(const u_t& u, const Q_t& Q, const ros::Time& stamp, const ModelPtr& model = nullptr)
+     142             :     {
+     143         400 :       const info_t info(stamp, u, Q, model);
+     144             :       // find the next point in the history buffer
+     145         200 :       const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), info, &Repredictor<Model>::earlier);
+     146             :       // add the point to the history buffer
+     147         200 :       const auto added = addInfo(info, next_it);
+     148             :       // update all measurements following the newly added system input up to the next system input
+     149         200 :       if (added != std::end(m_history))
+     150        5816 :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     151        5616 :           it->updateUsing(info);
+     152         200 :     }
+     153             : 
+     154             :     /*!
+     155             :      * \brief Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
+     156             :      *
+     157             :      * \param u      The system input vector to be added.
+     158             :      * \param Q      The process noise covariance matrix.
+     159             :      * \param stamp  Time stamp of the input vector and covariance matrix.
+     160             :      * \param model  Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default
+     161             :      * model specified in the constructor will be used.
+     162             :      *
+     163             :      * \note This is the variant of the method when reprediction is disabled and will function like a dumb LKF.
+     164             :      *
+     165             :      */
+     166             :     template<bool check=disable_reprediction>
+     167         212 :     std::enable_if_t<check> addInputChangeWithNoise(const u_t& u, const Q_t& Q, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     168             :     {
+     169         212 :       if (m_history.empty())
+     170           2 :         m_history.push_back({stamp});
+     171         212 :       m_history.front().u = u;
+     172         212 :       m_history.front().Q = Q;
+     173         212 :       m_history.front().stamp = stamp;
+     174         212 :       m_history.front().predict_model = model;
+     175         212 :     }
+     176             :     //}
+     177             : 
+     178             :     /* addInputChange() method //{ */
+     179             :     /*!
+     180             :      * \brief Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
+     181             :      *
+     182             :      * \param u      The system input vector to be added.
+     183             :      * \param stamp  Time stamp of the input vector and covariance matrix.
+     184             :      * \param model  Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default
+     185             :      * model specified in the constructor will be used.
+     186             :      *
+     187             :      * \note The system input vector will not be added if it is older than the oldest element in the history buffer.
+     188             :      *
+     189             :      */
+     190             :     template<bool check=disable_reprediction>
+     191             :     std::enable_if_t<!check> addInputChange(const u_t& u, const ros::Time& stamp, const ModelPtr& model = nullptr)
+     192             :     {
+     193             :       assert(!m_history.empty());
+     194             :       // find the next point in the history buffer
+     195             :       const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), stamp, &Repredictor<Model>::earlier);
+     196             :       // get the previous history point (or the first one to avoid out of bounds)
+     197             :       const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
+     198             :       // initialize a new history info point
+     199             :       const info_t info(stamp, u, prev_it->Q, model);
+     200             :       // add the point to the history buffer
+     201             :       const auto added = addInfo(info, next_it);
+     202             :       // update all measurements following the newly added system input up to the next system input
+     203             :       if (added != std::end(m_history))
+     204             :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     205             :           it->updateUsing(info);
+     206             :     }
+     207             : 
+     208             :     /*!
+     209             :      * \brief Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
+     210             :      *
+     211             :      * \param u      The system input vector to be added.
+     212             :      * \param stamp  Time stamp of the input vector and covariance matrix.
+     213             :      * \param model  Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default
+     214             :      * model specified in the constructor will be used.
+     215             :      *
+     216             :      * \note This is the variant of the method when reprediction is disabled and will function like a dumb LKF.
+     217             :      *
+     218             :      */
+     219             :     template<bool check=disable_reprediction>
+     220          93 :     std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     221             :     {
+     222          93 :       if (m_history.empty())
+     223           0 :         m_history.push_back({stamp});
+     224          93 :       m_history.front().u = u;
+     225          93 :       m_history.front().stamp = stamp;
+     226          93 :       m_history.front().predict_model = model;
+     227          93 :     }
+     228             :     //}
+     229             : 
+     230             :     /* addProcessNoiseChange() method //{ */
+     231             :     /*!
+     232             :      * \brief Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
+     233             :      *
+     234             :      * \param Q      The process noise covariance matrix.
+     235             :      * \param stamp  Time stamp of the input vector and covariance matrix.
+     236             :      * \param model  Optional pointer to a specific Model to be used with this covariance matrix (eg. mapping it to different states). If it equals to nullptr,
+     237             :      * the default model specified in the constructor will be used.
+     238             :      *
+     239             :      * \note The new element will not be added if it is older than the oldest element in the history buffer.
+     240             :      *
+     241             :      */
+     242             :     template<bool check=disable_reprediction>
+     243             :     std::enable_if_t<!check> addProcessNoiseChange(const Q_t& Q, const ros::Time& stamp, const ModelPtr& model = nullptr)
+     244             :     {
+     245             :       assert(!m_history.empty());
+     246             :       // find the next point in the history buffer
+     247             :       const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), stamp, &Repredictor<Model>::earlier);
+     248             :       // get the previous history point (or the first one to avoid out of bounds)
+     249             :       const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
+     250             :       // initialize a new history info point
+     251             :       const info_t info(stamp, prev_it->u, Q, model);
+     252             :       // add the point to the history buffer
+     253             :       const auto added = addInfo(info, next_it);
+     254             :       // update all measurements following the newly added system input up to the next system input
+     255             :       if (added != std::end(m_history))
+     256             :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     257             :           it->updateUsing(info);
+     258             :     }
+     259             : 
+     260             :     /*!
+     261             :      * \brief Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
+     262             :      *
+     263             :      * \param Q      The process noise covariance matrix.
+     264             :      * \param stamp  Time stamp of the input vector and covariance matrix.
+     265             :      * \param model  Optional pointer to a specific Model to be used with this covariance matrix (eg. mapping it to different states). If it equals to nullptr,
+     266             :      * the default model specified in the constructor will be used.
+     267             :      *
+     268             :      * \note This is the variant of the method when reprediction is disabled and will function like a dumb LKF.
+     269             :      *
+     270             :      */
+     271             :     template<bool check=disable_reprediction>
+     272             :     std::enable_if_t<check> addProcessNoiseChange(const Q_t& Q, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     273             :     {
+     274             :       if (m_history.empty())
+     275             :         m_history.push_back({stamp});
+     276             :       m_history.front().Q = Q;
+     277             :       m_history.front().stamp = stamp;
+     278             :       m_history.front().predict_model = model;
+     279             :     }
+     280             :     //}
+     281             : 
+     282             :     /* addMeasurement() method //{ */
+     283             :     /*!
+     284             :      * \brief Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full.
+     285             :      *
+     286             :      * \param z      The measurement vector to be added.
+     287             :      * \param R      The measurement noise covariance matrix, corresponding to the measurement vector.
+     288             :      * \param stamp  Time stamp of the measurement vector and covariance matrix.
+     289             :      * \param model  Optional pointer to a specific Model to be used with this measurement (eg. mapping it from different states). If it equals to nullptr, the
+     290             :      * default model specified in the constructor will be used.
+     291             :      *
+     292             :      * \note The measurement vector will not be added if it is older than the oldest element in the history buffer.
+     293             :      *
+     294             :      */
+     295             :     template<bool check=disable_reprediction>
+     296         100 :     std::enable_if_t<!check> addMeasurement(const z_t& z, const R_t& R, const ros::Time& stamp, const ModelPtr& model = nullptr, const double& meas_id = -1)
+     297             :     {
+     298         100 :       assert(!m_history.empty());
+     299             :       // helper variable for searching of the next point in the history buffer
+     300         100 :       const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), stamp, &Repredictor<Model>::earlier);
+     301             :       // get the previous history point (or the first one to avoid out of bounds)
+     302         100 :       const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
+     303             :       // initialize a new history info point
+     304         100 :       const info_t info(stamp, z, R, model, *prev_it, meas_id);
+     305             :       // add the point to the history buffer
+     306         100 :       addInfo(info, next_it);
+     307         100 :     }
+     308             : 
+     309             :     /*!
+     310             :      * \brief Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full.
+     311             :      *
+     312             :      * \param z      The measurement vector to be added.
+     313             :      * \param R      The measurement noise covariance matrix, corresponding to the measurement vector.
+     314             :      * \param stamp  Time stamp of the measurement vector and covariance matrix.
+     315             :      * \param model  Optional pointer to a specific Model to be used with this measurement (eg. mapping it from different states). If it equals to nullptr, the
+     316             :      * default model specified in the constructor will be used.
+     317             :      *
+     318             :      * \note This is the variant of the method when reprediction is disabled and will function like a dumb LKF.
+     319             :      *
+     320             :      */
+     321             :     template<bool check=disable_reprediction>
+     322         294 :     std::enable_if_t<check> addMeasurement(const z_t& z, const R_t& R, const ros::Time& stamp, const ModelPtr& model = nullptr, const double& meas_id = -1)
+     323             :     {
+     324         294 :       if (m_history.empty())
+     325           0 :         m_history.push_back({stamp});
+     326         294 :       auto& info = m_history.front();
+     327         294 :       const ros::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
+     328         294 :       const auto sc = predictTo(to_stamp);
+     329         294 :       info.z = z;
+     330         294 :       info.R = R;
+     331         294 :       info.stamp = to_stamp;
+     332         294 :       info.is_measurement = true;
+     333         294 :       info.meas_id = meas_id;
+     334         294 :       info.correct_model = model;
+     335         294 :       m_sc = correctFrom(sc, info);
+     336         294 :     }
+     337             :     //}
+     338             : 
+     339             :   public:
+     340             :     /* constructor //{ */
+     341             : 
+     342             :     /*!
+     343             :      * \brief The main constructor.
+     344             :      *
+     345             :      * Initializes the Repredictor with the necessary initial and default values.
+     346             :      *
+     347             :      * \param x0             Initial state.
+     348             :      * \param P0             Covariance matrix of the initial state uncertainty.
+     349             :      * \param u0             Initial system input.
+     350             :      * \param Q0             Default covariance matrix of the process noise.
+     351             :      * \param t0             Time stamp of the initial state.
+     352             :      * \param model          Default prediction and correction model.
+     353             :      * \param hist_len       Length of the history buffer for system inputs and measurements.
+     354             :      */
+     355           3 :     Repredictor(const x_t& x0, const P_t& P0, const u_t& u0, const Q_t& Q0, const ros::Time& t0, const ModelPtr& model, const unsigned hist_len)
+     356           3 :         : m_sc{x0, P0}, m_default_model(model), m_history(history_t(hist_len))
+     357             :     {
+     358           3 :       assert(hist_len > 0);
+     359           3 :       addInputChangeWithNoise(u0, Q0, t0, model);
+     360           3 :     };
+     361             : 
+     362             :     /*!
+     363             :      * \brief Empty constructor.
+     364             :      *
+     365             :      */
+     366             :     Repredictor(){};
+     367             : 
+     368             :     /*!
+     369             :      * \brief Variation of the constructor for cases without a system input.
+     370             :      *
+     371             :      * Initializes the Repredictor with the necessary initial and default values.
+     372             :      * Assumes the system input is zero at t0.
+     373             :      *
+     374             :      * \param x0             Initial state.
+     375             :      * \param P0             Covariance matrix of the initial state uncertainty.
+     376             :      * \param Q0             Default covariance matrix of the process noise.
+     377             :      * \param t0             Time stamp of the initial state.
+     378             :      * \param model          Default prediction and correction model.
+     379             :      * \param hist_len       Length of the history buffer for system inputs and measurements.
+     380             :      */
+     381             :     Repredictor(const x_t& x0, const P_t& P0, const Q_t& Q0, const ros::Time& t0, const ModelPtr& model, const unsigned hist_len)
+     382             :         : m_sc{x0, P0}, m_default_model(model), m_history(history_t(hist_len))
+     383             :     {
+     384             :       assert(hist_len > 0);
+     385             :       const u_t u0{0};
+     386             :       addInputChangeWithNoise(u0, Q0, t0, model);
+     387             :     };
+     388             :     //}
+     389             : 
+     390             :   protected:
+     391             :     // state and covariance corresponding to the oldest element in the history buffer
+     392             :     statecov_t m_sc;
+     393             :     // default model to use for updates
+     394             :     ModelPtr m_default_model;
+     395             : 
+     396             :   private:
+     397             :     /* helper structs and usings //{ */
+     398             : 
+     399             :     struct info_t
+     400             :     {
+     401             :       ros::Time stamp;
+     402             : 
+     403             :       // system input-related information
+     404             :       u_t u;
+     405             :       Q_t Q;
+     406             :       ModelPtr predict_model;
+     407             : 
+     408             :       // measurement-related information (unused in case is_measurement=false)
+     409             :       z_t z;
+     410             :       R_t R;
+     411             :       ModelPtr correct_model;
+     412             :       bool is_measurement;
+     413             :       int meas_id;
+     414             : 
+     415             :       // constructor for a dummy info (for searching in the history)
+     416         633 :       info_t(const ros::Time& stamp) : stamp(stamp), is_measurement(false){};
+     417             : 
+     418             :       // constructor for a system input
+     419         200 :       info_t(const ros::Time& stamp, const u_t& u, const Q_t& Q, const ModelPtr& model)
+     420         200 :           : stamp(stamp), u(u), Q(Q), predict_model(model), is_measurement(false){};
+     421             : 
+     422             :       // constructor for a measurement
+     423         100 :       info_t(const ros::Time& stamp, const z_t& z, const R_t& R, const ModelPtr& model, const info_t& prev_info, const int& meas_id)
+     424         100 :           : stamp(stamp), z(z), R(R), correct_model(model), is_measurement(true), meas_id(meas_id)
+     425             :       {
+     426         100 :         updateUsing(prev_info);
+     427         100 :       };
+     428             : 
+     429             :       // copy system input-related information from a previous info
+     430        5716 :       void updateUsing(const info_t& info)
+     431             :       {
+     432        5716 :         u = info.u;
+     433        5716 :         Q = info.Q;
+     434        5716 :         predict_model = info.predict_model;
+     435        5716 :       };
+     436             :     };
+     437             : 
+     438             : 
+     439             :     //}
+     440             : 
+     441             :   protected:
+     442             :     using history_t = boost::circular_buffer<info_t>;
+     443             :     // the history buffer
+     444             :     history_t m_history;
+     445             : 
+     446             :     // | ---------------- helper debugging methods ---------------- |
+     447             :     /* checkMonotonicity() method //{ */
+     448             :     template <typename T>
+     449             :     bool checkMonotonicity(const T& buf)
+     450             :     {
+     451             :       if (buf.empty())
+     452             :         return true;
+     453             :       for (auto it = std::begin(buf) + 1; it != std::end(buf); it++)
+     454             :         if (earlier(*it, *(it - 1)))
+     455             :           return false;
+     456             :       return true;
+     457             :     }
+     458             :     //}
+     459             : 
+     460             :     /* printBuffer() method //{ */
+     461             :     template <typename T>
+     462             :     void printBuffer(const T& buf)
+     463             :     {
+     464             :       if (buf.empty())
+     465             :         return;
+     466             :       const auto first_stamp = buf.front().stamp;
+     467             :       std::cerr << "first stamp: " << first_stamp << std::endl;
+     468             :       for (size_t it = 0; it < buf.size(); it++)
+     469             :       {
+     470             :         std::cerr << it << ":\t" << buf.at(it).stamp - first_stamp << std::endl;
+     471             :       }
+     472             :     }
+     473             :     //}
+     474             : 
+     475             :   private:
+     476             :     // | -------------- helper implementation methods ------------- |
+     477             : 
+     478             :     /* addInfo() method //{ */
+     479         300 :     typename history_t::iterator addInfo(const info_t& info, const typename history_t::iterator& next_it)
+     480             :     {
+     481             :       // check if the new element would be added before the first element of the history buffer and ignore it if so
+     482         300 :       if (next_it == std::begin(m_history) && !m_history.empty())
+     483             :       {
+     484           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[Repredictor]: Added history point is older than the oldest by "
+     485             :                                           << (next_it->stamp - info.stamp).toSec()
+     486             :                                           << "s. Ignoring it! Consider increasing the history buffer size (currently: " << m_history.size() << ")");
+     487           0 :         return std::end(m_history);
+     488             :       }
+     489             : 
+     490             :       // check if adding a new element would throw out the oldest one
+     491         300 :       if (m_history.size() == m_history.capacity())
+     492             :       {  // if so, first update m_sc
+     493             :         // figure out, what will be the oldest element in the buffer after inserting the newly received one
+     494           0 :         auto new_oldest = m_history.front();
+     495           0 :         if (m_history.size() == 1 || (m_history.size() > 1 && info.stamp > m_history.at(0).stamp && info.stamp < m_history.at(1).stamp))
+     496             :         {
+     497           0 :           new_oldest = info;
+     498           0 :         } else if (m_history.size() > 1)
+     499             :         {
+     500           0 :           new_oldest = m_history.at(1);
+     501             :         }
+     502             :         // update m_sc to the time of the new oldest element (after inserting the new element)
+     503           0 :         m_sc = predictTo(new_oldest.stamp);
+     504             :         // insert the new element into the history buffer, causing the original oldest element to be removed
+     505             :       }
+     506             : 
+     507             :       // add the new point finally
+     508         300 :       const auto ret = m_history.insert(next_it, info);
+     509             :       /* debug check //{ */
+     510             : 
+     511             : #ifdef REPREDICTOR_DEBUG
+     512             :       if (!checkMonotonicity(m_history))
+     513             :       {
+     514             :         std::cerr << "History buffer is not monotonous after modification!" << std::endl;
+     515             :       }
+     516             :       std::cerr << "Added info (" << m_history.size() << " total)" << std::endl;
+     517             : #endif
+     518             : 
+     519             :       //}
+     520         300 :       return ret;
+     521             :     }
+     522             :     //}
+     523             : 
+     524             :     /* earlier() method //{ */
+     525        2047 :     static bool earlier(const info_t& ob1, const info_t& ob2)
+     526             :     {
+     527        2047 :       return ob1.stamp < ob2.stamp;
+     528             :     }
+     529             :     //}
+     530             : 
+     531             :     /* predictFrom() method //{ */
+     532       16640 :     statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const ros::Time& from_stamp, const ros::Time& to_stamp)
+     533             :     {
+     534       33280 :       const auto model = inpt.predict_model == nullptr ? m_default_model : inpt.predict_model;
+     535       16640 :       const auto dt = (to_stamp - from_stamp).toSec();
+     536       33280 :       return model->predict(sc, inpt.u, inpt.Q, dt);
+     537             :     }
+     538             :     //}
+     539             : 
+     540             :     /* correctFrom() method //{ */
+     541        5444 :     statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
+     542             :     {
+     543        5444 :       assert(meas.is_measurement);
+     544       10888 :       const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
+     545        5444 :       auto sc_tmp = sc;
+     546       10888 :       return model->correct(sc_tmp, meas.z, meas.R);
+     547             :     }
+     548             :     //}
+     549             :   };
+     550             : }  // namespace mrs_lib
+     551             : 
+     552             : 
+     553             : #endif  // REPREDICTOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.overview.html b/mrs_lib/include/mrs_lib/repredictor.h.gcov.overview.html new file mode 100644 index 0000000000..a170247acb --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.overview.html @@ -0,0 +1,159 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.png b/mrs_lib/include/mrs_lib/repredictor.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..aa92389ebb5f6d1f6a537e734264aed4e4dcf929 GIT binary patch literal 2387 zcmV-Z39R;sP)@QT39+@`)}ol^*Y#&#K6GpjduqED3YwtlAkO&=ag$p%e1)x z-EG4Kh|&$1r|=D+594vK32+YQC+%*TBz`Dvzdqcm0}yRzNOXzsB({2S1N4%H4W%He zcS9niNl_x{?MkRYeS<83+owbrxBoy~CE~V1^X;$!-aaJa9N&|mWDsG8OJx%AnM#Gr}MKUa$Cml!nN$LMIhy#yjdy+t-K@DKM;xB_%e2 zn69-8hyq0VpsG?pdE-ujDhNqKGKOe12`Yd@y#^we=tpaJD4k?^WXk|!i=a*%=`}e8 zwQNnI%Y?EQv>eB*=8=WYTy`&MjpV{6eH7!KXE_ zZb&$t&Ze#AmEKEzIo$xu$d6cE@EIBBYt)JT>; zxAs@t^?G~x(`g&vwfDmv;KJS|1ELPX%oBk)qjS`k2( z9Nhvxo!BalJ9FzzLEg6u>_(~mTlGc)Y%Fbz@zh$`+OxF9(~%0uIz-a}t3}b|_PVID zlLR^qcW8Cnm58Sp5lzH{K*e;w3W{#ruBF%XWhny*MSIO*YZ7^g@?PF*l8p+LxuGS+ zHKVEdfK!pq5ysnJA-30|QHn)h*p@WxjkPBjNg9Wwoe?+Ce#h(Y90UoH~$j{EG?&Zqz`=1c%h>+ZZCXU9!_nNjz|4keNY1> z7sFU{#hh!a@omwTxY`*3N)?{&t&(_5U^r|&Fz*3nFa|rY;#9@LUT7j0N1eUcf_`m! zOlf27I!^+{o2Nj0;`&bL>W+4^UQ=tFL~vu?+lRd8+}I@bwP_J>SVsI+B^d`7{Lgt zPUR;VNoZgZ_Fy!zA@!)@>kLPR$9b0xkGi@!0JVm1<~GgYj1($p(PF7 z;!^kxckvVkY%rQ-1uPrzMGK!I46V~el4c>NkR*)|g3B)V2zr1YvZV1FDZUyhJ~;z8 zm{pLsx6djZZGAXdcRkB4ua*7WZk|$%#dLXyaa*GF^f~}|yz^o|mvXQp{d-K_)2w@$kB?YRrdks-cCe}pj>NE6qs>)3y3mk?j$M_iUMu zb-XSdV!N>yqtPR|*_O|^0GTG`Djj!qllB3|lMgkgA)pgsbkKa#NlC=`z&_Gu|0mC+ zsVjI%`y&NtPM7(3S{Gx4^vgraPmL66W=V9c=OH!Ut)PijXR^55whHE?EVNTPK4Y8) zQG3vv%mVCB&Mt1Rdiq?7->0*d_F8Q4_$8WWl;DvL7Znr92$M49B;vY%w^X4X{-VLe5xk>Z`5QD%-wKULGV1^}-3 zbJi-Kpd>+(GdRuOSgAfk2W=YCX(Xc#>H8}D%$s71B#os>9nIzAJPt+w5+0-H;z$J( zK=+Y6S0nKxHFC+PcyPAj5ksLPh6B?6UYA~m{v|-E6(-V*$kACVyrR7`RiJEopN`#e zG{`^uw3D9!50A2{FXk($ZpL|5;n9p9le6q%|3^7OGw{%(I=U^su;_sF>#!_P<=ET2Hy811}w78vBL0uX6BD~q7E|_+hw6RXA%%;Br$|Q|zg#=D3s@uYEa1XW2#Hul#oTyL1<0Za-kV@WtPuXd%mlN|iH{ zz9{9vvmYW4i3lkhA0A z2DZLbwksBXSNGv4r+K-VRmw>+!WIPRz*{{y3d#5}|UW#|9^002ovPDHLk FV1iP8dXNAB literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html new file mode 100644 index 0000000000..fbab9b54ae --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-25 05:42:57Functions:040.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.h +
0.0%
+ 		0.0 %0 / 20.0 %0 / 1
polygon.h +
0.0%
+ 		0.0 %0 / 60.0 %0 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html new file mode 100644 index 0000000000..90a5a9a550 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-25 05:42:57Functions:040.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.h +
0.0%
+ 		0.0 %0 / 20.0 %0 / 1
polygon.h +
0.0%
+ 		0.0 %0 / 60.0 %0 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/index-detail.html b/mrs_lib/include/mrs_lib/safety_zone/index-detail.html new file mode 100644 index 0000000000..4fcd4930ce --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-25 05:42:57Functions:040.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
polygon.h +
0.0%
+ 		0.0 %0 / 60.0 %0 / 3
safety_zone.h +
0.0%
+ 		0.0 %0 / 20.0 %0 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html b/mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html new file mode 100644 index 0000000000..eb4607a566 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-25 05:42:57Functions:040.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.h +
0.0%
+ 		0.0 %0 / 20.0 %0 / 1
polygon.h +
0.0%
+ 		0.0 %0 / 60.0 %0 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html b/mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html new file mode 100644 index 0000000000..a0a762c882 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-25 05:42:57Functions:040.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.h +
0.0%
+ 		0.0 %0 / 20.0 %0 / 1
polygon.h +
0.0%
+ 		0.0 %0 / 60.0 %0 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/index.html b/mrs_lib/include/mrs_lib/safety_zone/index.html new file mode 100644 index 0000000000..6720af9cec --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-25 05:42:57Functions:040.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
polygon.h +
0.0%
+ 		0.0 %0 / 60.0 %0 / 3
safety_zone.h +
0.0%
+ 		0.0 %0 / 20.0 %0 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html new file mode 100644 index 0000000000..dbf2b8224c --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-04-25 05:42:57Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Polygon::ExtraVertices::what() const0
mrs_lib::Polygon::WrongNumberOfColumns::what() const0
mrs_lib::Polygon::WrongNumberOfVertices::what() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html new file mode 100644 index 0000000000..859142869e --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-04-25 05:42:57Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Polygon::ExtraVertices::what() const0
mrs_lib::Polygon::WrongNumberOfColumns::what() const0
mrs_lib::Polygon::WrongNumberOfVertices::what() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.frameset.html new file mode 100644 index 0000000000..8b592d08da --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html new file mode 100644 index 0000000000..a389b59a23 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-04-25 05:42:57Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: TomasFormat
+       2             : #ifndef MRS_LIB_POLYGON_H
+       3             : #define MRS_LIB_POLYGON_H
+       4             : 
+       5             : #include <ros/ros.h>
+       6             : #include <eigen3/Eigen/Eigen>
+       7             : #include <vector>
+       8             : #include <geometry_msgs/Point.h>
+       9             : #include <mrs_lib/safety_zone/line_operations.h>
+      10             : 
+      11             : namespace mrs_lib
+      12             : {
+      13             : class Polygon {
+      14             : public:
+      15             :   Polygon(const Eigen::MatrixXd vertices);
+      16             : 
+      17             :   bool isPointInside(const double px, const double py);
+      18             :   bool doesSectionIntersect(const double startX, const double startY, const double endX, const double endY);
+      19             :   bool isClockwise();
+      20             :   void inflateSelf(double amount);
+      21             : 
+      22             :   std::vector<geometry_msgs::Point> getPointMessageVector(const double z);
+      23             : 
+      24             : private:
+      25             :   Eigen::MatrixXd vertices;
+      26             : 
+      27             : public:
+      28             :   // exceptions
+      29             :   struct WrongNumberOfVertices : public std::exception
+      30             :   {
+      31           0 :     const char* what() const throw() {
+      32           0 :       return "Polygon: wrong number of vertices was supplied!";
+      33             :     }
+      34             :   };
+      35             : 
+      36             :   struct WrongNumberOfColumns : public std::exception
+      37             :   {
+      38           0 :     const char* what() const throw() {
+      39           0 :       return "Polygon: wrong number of colums, it should be =2!";
+      40             :     }
+      41             :   };
+      42             : 
+      43             :   struct ExtraVertices : public std::exception
+      44             :   {
+      45           0 :     const char* what() const throw() {
+      46           0 :       return "Polygon: useless vertices detected, polygon methods may break!";
+      47             :     }
+      48             :   };
+      49             : };
+      50             : }  // namespace mrs_lib
+      51             : 
+      52             : #endif  // MRS_LIB_POLYGON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.overview.html b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.overview.html new file mode 100644 index 0000000000..3a568cf9a5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.overview.html @@ -0,0 +1,33 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.png b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..69c6e8249f4419ea60e73f57611e818597436175 GIT binary patch literal 337 zcmeAS@N?(olHy`uVBq!ia0vp^0YGfR!VDzeM=iPpq$C1-LR|m<|Gx?dmcNgUef6J# zVHHpuOr7)IycEdhEbxddW?X?_wfUrhg7-aL978-h&xUsL9#P=QHuETJ;yv_l zwT!=jKeuP;+m|{aEh%#nda48j6jdK=WDT9?W-5Hv!)wCVGcHRjR9F4&?V8P&wrH0l zYv;tQ{4dKZToN)kHmM>qTYD`~>&gdj!ff zE*HJCIw^nk`X48Z_Xu(sh{@jPaWKwMisF9|!!gJGLnX`mbpoC13bZDEZu)%9qTFR) zh=ryc|B=)~3H(aB&40A3ZBDmKf0Tc7Q$_t@YPE{xr-x#T&EFW_?r3;s-?6d8WIJoo b+20JO`5O;uuKc40^eTg=tDnm{r-UW|8XSfQ literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html new file mode 100644 index 0000000000..7bdcabc669 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-04-25 05:42:57Functions:010.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SafetyZone::BorderError::what() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html new file mode 100644 index 0000000000..e13c4e01e4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-04-25 05:42:57Functions:010.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SafetyZone::BorderError::what() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.frameset.html new file mode 100644 index 0000000000..b372dc445a --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html new file mode 100644 index 0000000000..d180d0c21d --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html @@ -0,0 +1,121 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-04-25 05:42:57Functions:010.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: TomasFormat
+       2             : #ifndef MRS_LIB_SAFETYZONE_H
+       3             : #define MRS_LIB_SAFETYZONE_H
+       4             : 
+       5             : #include <mrs_lib/safety_zone/polygon.h>
+       6             : #include <visualization_msgs/Marker.h>
+       7             : #include <eigen3/Eigen/Eigen>
+       8             : #include <vector>
+       9             : 
+      10             : namespace mrs_lib
+      11             : {
+      12             : class SafetyZone {
+      13             : public:
+      14             :   SafetyZone(Polygon outerBorder);
+      15             :   ~SafetyZone();
+      16             : 
+      17             :   SafetyZone(const Eigen::MatrixXd& outerBorderMatrix);
+      18             : 
+      19             :   bool isPointValid(const double px, const double py);
+      20             :   bool isPathValid(const double p1x, const double p1y, const double p2x, const double p2y);
+      21             : 
+      22             :   Polygon getBorder();
+      23             : 
+      24             : private:
+      25             :   Polygon* outerBorder;
+      26             : 
+      27             : public:
+      28             :   struct BorderError : public std::exception
+      29             :   {
+      30           0 :     const char* what() const throw() {
+      31           0 :       return "SafeZone: Wrong configuration for the border";
+      32             :     }
+      33             :   };
+      34             : };
+      35             : }  // namespace mrs_lib
+      36             : 
+      37             : #endif  // MRS_LIB_SAFETYZONE_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.overview.html b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.overview.html new file mode 100644 index 0000000000..20ffd7de34 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.overview.html @@ -0,0 +1,30 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.png b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1b39b2911483785bd7a8e252290372648cd02641 GIT binary patch literal 280 zcmeAS@N?(olHy`uVBq!ia0vp^0YI$E!VDzUFBHE4QW60^A+G=b|6c_J%iqVwzWUF= zunH&+rp|e9UJ7J$7I;J!GcfQS0b$0e+I-SL!DXH$Xhoi1$?<^|$XXd{aqZ z@a?+Nrut{KJqP__-W)l)eS=`_BawZ+ioN1V8Co_XvbSaKZg6;E-mvA~rPO(wMHgk6 VNwG-e^8j7U;OXk;vd$@?2>?zzaFhT5 literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html b/mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html new file mode 100644 index 0000000000..1b4106a3e0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-04-25 05:42:57Functions:1333.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ScopeTimerLogger::loggingEnabled()0
mrs_lib::ScopeTimerLogger::shouldLog()0
mrs_lib::ScopeTimer::time_point::time_point(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5214781
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.func.html b/mrs_lib/include/mrs_lib/scope_timer.h.func.html new file mode 100644 index 0000000000..649b67a218 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-04-25 05:42:57Functions:1333.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ScopeTimer::time_point::time_point(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5214781
mrs_lib::ScopeTimerLogger::loggingEnabled()0
mrs_lib::ScopeTimerLogger::shouldLog()0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.frameset.html new file mode 100644 index 0000000000..bb60579787 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.gcov.html b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.html new file mode 100644 index 0000000000..88ecb73b66 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.html @@ -0,0 +1,248 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-04-25 05:42:57Functions:1333.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :  *   \brief Simple timer which times a duration of its scope, with additional optional checkpoints.
+       3             :  *   \author Daniel Hert - hertdani@fel.cvut.cz
+       4             :  */
+       5             : #ifndef SCOPE_TIMER_H
+       6             : #define SCOPE_TIMER_H
+       7             : 
+       8             : #include <ros/ros.h>
+       9             : #include <chrono>
+      10             : #include <iostream>
+      11             : #include <fstream>
+      12             : #include <iomanip>
+      13             : #include <mutex>
+      14             : /* #include <ctime> */
+      15             : 
+      16             : namespace mrs_lib
+      17             : {
+      18             : 
+      19             : /*//{ ScopeTimerLogger class */
+      20             : 
+      21             : /**
+      22             :  * @brief Simple file logger of scope timer and its checkpoints
+      23             :  */
+      24             : class ScopeTimerLogger {
+      25             : 
+      26             : public:
+      27             :   /**
+      28             :    * @brief The basic constructor with a user-defined path to the logger file, enable flag and float-logging precision
+      29             :    */
+      30             :   ScopeTimerLogger(const std::string& logfile, const bool enable_logging = true, const int log_precision = 10);
+      31             : 
+      32             :   /**
+      33             :    * @brief The basic destructor which closes the logging file
+      34             :    */
+      35             :   ~ScopeTimerLogger();
+      36             : 
+      37             :   /**
+      38             :    * @brief Returns true if a non-empty path to a logger file was given by the user
+      39             :    */
+      40           0 :   bool shouldLog() {
+      41           0 :     return _should_log_;
+      42             :   }
+      43             : 
+      44             :   /**
+      45             :    * @brief Returns true if the enable flag was set to true, a non-empty path to a logger file was given by the user and the logger file stream was successfully
+      46             :    * opened.
+      47             :    */
+      48           0 :   bool loggingEnabled() {
+      49           0 :     return _logging_enabled_;
+      50             :   }
+      51             : 
+      52             :   /**
+      53             :    * @brief Returns the path to the log.
+      54             :    */
+      55             :   std::string getLogFilepath() {
+      56             :     return _log_filepath_;
+      57             :   }
+      58             : 
+      59             :   using chrono_tp = std::chrono::time_point<std::chrono::steady_clock>;
+      60             : 
+      61             :   /**
+      62             :    * @brief Writes the time data of the given scope and empty checkpoints into the logger stream.
+      63             :    */
+      64             :   void log(const std::string& scope, const chrono_tp& time_start, const chrono_tp& time_end);
+      65             : 
+      66             :   /**
+      67             :    * @brief Writes the time data of the given scope and checkpoint labels into the logger stream.
+      68             :    */
+      69             :   void log(const std::string& scope, const std::string& label_from, const std::string& label_to, const chrono_tp& time_start, const chrono_tp& time_end);
+      70             : 
+      71             : private:
+      72             :   bool          _logging_enabled_ = false;
+      73             :   bool          _should_log_      = false;
+      74             :   std::string   _log_filepath_;
+      75             :   std::ofstream _logstream_;
+      76             :   std::mutex    _mutex_logstream_;
+      77             : };
+      78             : 
+      79             : /*//}*/
+      80             : 
+      81             : /**
+      82             :  * @brief Simple timer which will time a duration of a scope and checkpoints inside the scope in ros time and std::chrono time.
+      83             :  */
+      84             : class ScopeTimer {
+      85             : public:
+      86             :   /* time_point() helper struct //{ */
+      87             :   struct time_point
+      88             :   {
+      89             :     using chrono_tp = std::chrono::time_point<std::chrono::steady_clock>;
+      90             : 
+      91             :   public:
+      92     5214781 :     time_point(const std::string& label) : ros_time(ros::Time::now()), chrono_time(std::chrono::steady_clock::now()), label(label) {
+      93     5214101 :     }
+      94             : 
+      95             :     time_point(const std::string& label, const ros::Time& ros_time) : ros_time(ros_time), label(label) {
+      96             :       // helper types to make the code slightly more readable
+      97             :       using float_seconds   = std::chrono::duration<float>;
+      98             :       using chrono_duration = std::chrono::steady_clock::duration;
+      99             :       // prepare ros and chrono current times to minimize their difference
+     100             :       const auto ros_now    = ros::Time::now();
+     101             :       const auto chrono_now = std::chrono::steady_clock::now();
+     102             :       // calculate how old is ros_time
+     103             :       const auto ros_dt = ros_now - ros_time;
+     104             :       // cast ros_dt to chrono type
+     105             :       const auto chrono_dt = std::chrono::duration_cast<chrono_duration>(float_seconds(ros_dt.toSec()));
+     106             :       // calculate ros_time in chrono time and set it to chrono_time
+     107             :       chrono_time = chrono_now - chrono_dt;
+     108             :     }
+     109             : 
+     110             :     ros::Time   ros_time;
+     111             :     chrono_tp   chrono_time;
+     112             :     std::string label;
+     113             :   };
+     114             :   //}
+     115             : 
+     116             : public:
+     117             :   /**
+     118             :    * @brief The basic constructor with a user-defined label of the timer, throttled period and file logger.
+     119             :    */
+     120             :   ScopeTimer(const std::string& label, const ros::Duration& throttle_period = ros::Duration(0), const bool enable = true,
+     121             :              const std::shared_ptr<ScopeTimerLogger> scope_timer_logger = nullptr);
+     122             : 
+     123             :   /**
+     124             :    * @brief The basic constructor with a user-defined label of the timer and a pre-start time, which will also be measured.
+     125             :    */
+     126             :   ScopeTimer(const std::string& label, const time_point& tp0, const ros::Duration& throttle_period = ros::Duration(0), const bool enable = true,
+     127             :              const std::shared_ptr<ScopeTimerLogger> scope_timer_logger = nullptr);
+     128             : 
+     129             :   /**
+     130             :    * @brief The basic constructor with a user-defined label of the timer and file logger.
+     131             :    */
+     132             :   ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable = true);
+     133             : 
+     134             :   /**
+     135             :    * @brief Checkpoint, prints the time passed until the point this function is called.
+     136             :    */
+     137             :   void checkpoint(const std::string& label);
+     138             : 
+     139             :   /**
+     140             :    * @brief Getter for scope timer lifetime
+     141             :    *
+     142             :    * @return lifetime as floating point milliseconds
+     143             :    */
+     144             :   float getLifetime();
+     145             : 
+     146             :   /**
+     147             :    * @brief The basic destructor which prints out or logs the scope times, if enabled.
+     148             :    */
+     149             :   ~ScopeTimer();
+     150             : 
+     151             : private:
+     152             :   std::string             _timer_label_;
+     153             :   bool                    _enable_print_or_log;
+     154             :   ros::Duration           _throttle_period_;
+     155             :   std::vector<time_point> checkpoints;
+     156             : 
+     157             :   std::shared_ptr<ScopeTimerLogger> _logger_ = nullptr;
+     158             : 
+     159             :   static std::unordered_map<std::string, ros::Time> last_print_times;
+     160             : };
+     161             : 
+     162             : }  // namespace mrs_lib
+     163             : 
+     164             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html new file mode 100644 index 0000000000..82f8580772 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html @@ -0,0 +1,61 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.gcov.png b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..d57ab8c0703ba94fdd794b60b6475e555513d985 GIT binary patch literal 777 zcmV+k1NQuhP)<}7j3 zxrRs>_JFGeLNsPr?+Qn&IfG`S=o<3?vJ)hU zjAZ~dhqBB;Jj)NpJoctB;8UAh9a`dqF$4OxIgSP-PXv;_1F;fDX6CW)&;6w5qe17B@U0r+Y6Q1l9h?T!Z=(6(!i`f=Z1Ee9liJg|S=kHM1_;Lw^erM{3NIurD< zjRu378I8)E*k~ZS9J4GuCF1Z?RNL{#nRt26U&cbf5JiYA(~Lam@z zr^t`RCZ%smU>U`(w)(it^w$NeAqx5rR{-KsMk + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2024-04-25 05:42:57Functions:2424100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler()1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler_impl()1
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler()2
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler()2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::~ServiceClientHandler_impl()2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::~ServiceClientHandler()4
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()94
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()94
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()95
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()95
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()188
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()188
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()188
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()188
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()190
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()376
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()566
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()566
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()1032
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()1032
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()1132
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1970
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/service_client_handler.h.func.html b/mrs_lib/include/mrs_lib/service_client_handler.h.func.html new file mode 100644 index 0000000000..deace2542a --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2024-04-25 05:42:57Functions:2424100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler()2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::~ServiceClientHandler()4
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler()1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler()2
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()95
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()190
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()94
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()188
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()94
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()188
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()188
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()376
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()566
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()1132
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()1032
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1970
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::~ServiceClientHandler_impl()2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler_impl()1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()95
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()188
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()566
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()1032
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.frameset.html new file mode 100644 index 0000000000..6bee985b3e --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html new file mode 100644 index 0000000000..e3a9036b8e --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html @@ -0,0 +1,327 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2024-04-25 05:42:57Functions:2424100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Defines ServiceClientHandler and related convenience classes for upgrading the ROS service client
+       3             :      \author Tomas Baca - tomas.baca@fel.cvut.cz
+       4             :  */
+       5             : #ifndef SERVICE_CLIENT_HANDLER_H
+       6             : #define SERVICE_CLIENT_HANDLER_H
+       7             : 
+       8             : #include <ros/ros.h>
+       9             : #include <ros/package.h>
+      10             : 
+      11             : #include <string>
+      12             : #include <future>
+      13             : #include <mutex>
+      14             : 
+      15             : namespace mrs_lib
+      16             : {
+      17             : 
+      18             : /* class ServiceClientHandler_impl //{ */
+      19             : 
+      20             : /**
+      21             :  * @brief implementation of the service client handler
+      22             :  */
+      23             : template <class ServiceType>
+      24             : class ServiceClientHandler_impl {
+      25             : 
+      26             : public:
+      27             :   /**
+      28             :    * @brief default constructor
+      29             :    */
+      30             :   ServiceClientHandler_impl(void);
+      31             : 
+      32             :   /**
+      33             :    * @brief default destructor
+      34             :    */
+      35        2072 :   ~ServiceClientHandler_impl(void){};
+      36             : 
+      37             :   /**
+      38             :    * @brief constructor
+      39             :    *
+      40             :    * @param nh ROS node handler
+      41             :    * @param address service address
+      42             :    */
+      43             :   ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address);
+      44             : 
+      45             :   /**
+      46             :    * @brief "classic" synchronous service call
+      47             :    *
+      48             :    * @param srv data
+      49             :    *
+      50             :    * @return true when success
+      51             :    */
+      52             :   bool call(ServiceType& srv);
+      53             : 
+      54             :   /**
+      55             :    * @brief "classic" synchronous service call with repeats after an error
+      56             :    *
+      57             :    * @param srv data
+      58             :    * @param attempts how many attempts for the call
+      59             :    *
+      60             :    * @return  true when success
+      61             :    */
+      62             :   bool call(ServiceType& srv, const int& attempts);
+      63             : 
+      64             :   /**
+      65             :    * @brief "classic" synchronous service call with repeats after an error
+      66             :    *
+      67             :    * @param srv data
+      68             :    * @param attempts how many attempts for the call
+      69             :    * @param repeat_delay how long to wait before repeating the call
+      70             :    *
+      71             :    * @return  true when success
+      72             :    */
+      73             :   bool call(ServiceType& srv, const int& attempts, const double& repeat_delay);
+      74             : 
+      75             :   /**
+      76             :    * @brief asynchronous service call
+      77             :    *
+      78             :    * @param srv data
+      79             :    *
+      80             :    * @return future result
+      81             :    */
+      82             :   std::future<ServiceType> callAsync(ServiceType& srv);
+      83             : 
+      84             :   /**
+      85             :    * @brief asynchronous service call with repeates after an error
+      86             :    *
+      87             :    * @param srv data
+      88             :    * @param attempts how many attempts for the call
+      89             :    *
+      90             :    * @return future result
+      91             :    */
+      92             :   std::future<ServiceType> callAsync(ServiceType& srv, const int& attempts);
+      93             : 
+      94             :   /**
+      95             :    * @brief asynchronous service call with repeates after an error
+      96             :    *
+      97             :    * @param srv data
+      98             :    * @param attempts how many attempts for the call
+      99             :    * @param repeat_delay how long to wait before repeating the call
+     100             :    *
+     101             :    * @return future result
+     102             :    */
+     103             :   std::future<ServiceType> callAsync(ServiceType& srv, const int& attempts, const double& repeat_delay);
+     104             : 
+     105             : private:
+     106             :   ros::ServiceClient service_client_;
+     107             :   std::mutex         mutex_service_client_;
+     108             :   std::atomic<bool>  service_initialized_;
+     109             : 
+     110             :   std::string _address_;
+     111             : 
+     112             :   ServiceType async_data_;
+     113             :   int         async_attempts_;
+     114             :   double      async_repeat_delay_;
+     115             :   std::mutex  mutex_async_;
+     116             : 
+     117             :   ServiceType asyncRun(void);
+     118             : };
+     119             : 
+     120             : //}
+     121             : 
+     122             : /* class ServiceClientHandler //{ */
+     123             : 
+     124             : /**
+     125             :  * @brief user wrapper of the service client handler implementation
+     126             :  */
+     127             : template <class ServiceType>
+     128             : class ServiceClientHandler {
+     129             : 
+     130             : public:
+     131             :   /**
+     132             :    * @brief generic constructor
+     133             :    */
+     134        2072 :   ServiceClientHandler(void){};
+     135             : 
+     136             :   /**
+     137             :    * @brief generic destructor
+     138             :    */
+     139        4050 :   ~ServiceClientHandler(void){};
+     140             : 
+     141             :   /**
+     142             :    * @brief operator=
+     143             :    *
+     144             :    * @param other
+     145             :    *
+     146             :    * @return
+     147             :    */
+     148             :   ServiceClientHandler& operator=(const ServiceClientHandler& other);
+     149             : 
+     150             :   /**
+     151             :    * @brief copy constructor
+     152             :    *
+     153             :    * @param other
+     154             :    */
+     155             :   ServiceClientHandler(const ServiceClientHandler& other);
+     156             : 
+     157             :   /**
+     158             :    * @brief constructor
+     159             :    *
+     160             :    * @param nh ROS node handler
+     161             :    * @param address service address
+     162             :    */
+     163             :   ServiceClientHandler(ros::NodeHandle& nh, const std::string& address);
+     164             : 
+     165             :   /**
+     166             :    * @brief initializer
+     167             :    *
+     168             :    * @param nh ROS node handler
+     169             :    * @param address service address
+     170             :    */
+     171             :   void initialize(ros::NodeHandle& nh, const std::string& address);
+     172             : 
+     173             :   /**
+     174             :    * @brief "standard" synchronous call
+     175             :    *
+     176             :    * @param srv data
+     177             :    *
+     178             :    * @return true when success
+     179             :    */
+     180             :   bool call(ServiceType& srv);
+     181             : 
+     182             :   /**
+     183             :    * @brief "standard" synchronous call with repeats after failure
+     184             :    *
+     185             :    * @param srv data
+     186             :    * @param attempts how many attempts for the call
+     187             :    *
+     188             :    * @return true when success
+     189             :    */
+     190             :   bool call(ServiceType& srv, const int& attempts);
+     191             : 
+     192             :   /**
+     193             :    * @brief "standard" synchronous call with repeats after failure
+     194             :    *
+     195             :    * @param srv data
+     196             :    * @param attempts how many attempts for the call
+     197             :    * @param repeat_delay how long to wait before repeating the call
+     198             :    *
+     199             :    * @return true when success
+     200             :    */
+     201             :   bool call(ServiceType& srv, const int& attempts, const double& repeat_delay);
+     202             : 
+     203             :   /**
+     204             :    * @brief asynchronous call
+     205             :    *
+     206             :    * @param srv data
+     207             :    *
+     208             :    * @return future result
+     209             :    */
+     210             :   std::future<ServiceType> callAsync(ServiceType& srv);
+     211             : 
+     212             :   /**
+     213             :    * @brief asynchronous call with repeats after failure
+     214             :    *
+     215             :    * @param srv data
+     216             :    * @param attempts how many attemps for the call
+     217             :    *
+     218             :    * @return future result
+     219             :    */
+     220             :   std::future<ServiceType> callAsync(ServiceType& srv, const int& attempts);
+     221             : 
+     222             :   /**
+     223             :    * @brief asynchronous call with repeats after failure
+     224             :    *
+     225             :    * @param srv data
+     226             :    * @param attempts how many attemps for the call
+     227             :    * @param repeat_delay how long to wait before repeating the call
+     228             :    *
+     229             :    * @return future result
+     230             :    */
+     231             :   std::future<ServiceType> callAsync(ServiceType& srv, const int& attempts, const double& repeat_delay);
+     232             : 
+     233             : private:
+     234             :   std::shared_ptr<ServiceClientHandler_impl<ServiceType>> impl_;
+     235             : };
+     236             : 
+     237             : //}
+     238             : 
+     239             : }  // namespace mrs_lib
+     240             : 
+     241             : #include <mrs_lib/impl/service_client_handler.hpp>
+     242             : 
+     243             : #endif  // SERVICE_CLIENT_HANDLER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.overview.html b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.overview.html new file mode 100644 index 0000000000..49a39afd93 --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.overview.html @@ -0,0 +1,81 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.png b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1f77d70b09707c50b06bd68bf8ff5cf1bfc35fd8 GIT binary patch literal 689 zcmV;i0#5yjP)N=w&DXGh)NcNV9X*KZ$UKn;|9Ec z%=R(v35*q7{{(}P>|P0=Oz*;#7R(h3iU)aC#R`HW)W(Qamehh{>?1^!sFsFWp&A0= zl#G2^>Rz}35b_MrFN!_2nhZAqv?$0D9Q%2dM9)cT`LU)JW~ulYys+DxDG3S@U%%=2 zc^!@A@V`fbEty-H0YX5BxEB6O9f)}>iZ&+&RRO9CDDX2uRP%^l6CNgqlE-6^c0?PN zO3hag4uR$&yGSSDKo95*nm{t8^JIjJC+edVe#P(IwLAX1?$LnPil&ZeEnw=CG zpcSXwex4PXIo0g+5DGZqfJ)V*H)pd?qu9U_B1VF8%d?0Lc`QOKLs}OFZrE=H9@}rr zTcObK#eCxZ}&H!66aVyy(-h{|HNEpFbXNKO-z`xN3jR8f8VKE+FNub+IipVp_i z!kOv>x?VI^sDL(dU>HM!e zviA>ttw)Qf=aF8%WoBM~V}DrxF%a%gQp~*%y0OoS%tigjJ4a^Ljs1iWp51>eO}f6C zJ+S|nSuR#ks=9s6U}oMeBXe&5@vf2i_8+CHlfnG^gyt23f3-IVI|)b_J%|f~%vkyd XJM*UY61ZA500000NkvXXu0mjf!D=)% literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html new file mode 100644 index 0000000000..9b45a950e9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html @@ -0,0 +1,3044 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-04-25 05:42:57Functions:39074152.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcTfToWorld<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<Tester>(mrs_lib::SubscribeHandlerOptions const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const3
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::stop()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::lastMsgTime() const5
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)9
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::getMsg()22
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const90
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)91
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)91
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const91
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::uav_manager::UavManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::speed_tracker::SpeedTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::uav_manager::UavManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*)95
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcTfToWorld<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*)95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)95
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const95
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*)::{lambda()#1}::operator()() const95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const95
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)> const&)96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)96
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)98
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const98
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::HdgPassthrough>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfSource>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::HdgPassthrough>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const>)> const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)99
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::topicName[abi:cxx11]() const99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()100
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)100
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfSource>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)100
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)::{lambda()#1}::operator()() const100
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const108
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const116
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)150
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)150
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const150
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)> const&)181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)181
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const185
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)188
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const188
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)> const&)192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()194
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()198
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)> const&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)219
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::lastMsgTime() const260
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)270
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const270
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()272
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)282
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const282
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)> const&)290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)290
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)> const&)301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)301
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)> const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const313
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()354
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~SubscribeHandler()354
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()354
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()354
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()355
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()364
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const>)> const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const365
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)> const&)374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)374
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)> const&)376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)376
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()376
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()376
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()377
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()385
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()385
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()386
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const407
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::~SubscribeHandler()416
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()438
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()448
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()451
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()457
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()532
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()552
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)> const&)562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)562
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()564
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::~SubscribeHandler()566
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()730
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()738
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()751
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()752
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()814
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()814
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()853
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()1028
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1271
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()1833
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()2151
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2428
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2492
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()3753
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()13942
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const16455
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const17559
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const18482
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const18685
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const18711
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()19921
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const36861
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()44782
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const52850
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const66652
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()68533
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const70911
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const107071
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const143367
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const166115
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()183687
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::lastMsgTime() const207966
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()224352
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const242295
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const379107
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()388879
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::lastMsgTime() const410827
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const517469
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const529124
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()693451
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()708469
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const907275
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html new file mode 100644 index 0000000000..51f8c209ef --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html @@ -0,0 +1,3044 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-04-25 05:42:57Functions:39074152.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)> const&)374
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()377
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)91
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*)95
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::uav_manager::UavManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)91
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcTfToWorld<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcTfToWorld<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*)95
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()751
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)374
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()385
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::~SubscribeHandler()416
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()188
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()376
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)188
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()13942
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)> const&)181
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()385
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)150
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)150
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::~SubscribeHandler()566
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)181
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()354
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~SubscribeHandler()354
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()100
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()3753
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)> const&)96
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()355
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()451
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)96
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()183687
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)> const&)301
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()552
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::HdgPassthrough>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()853
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)301
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()708469
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)> const&)562
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1271
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)270
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfSource>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::HdgPassthrough>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()1833
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)562
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()354
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()354
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()272
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)> const&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()438
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()166111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)> const&)192
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()194
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)98
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()386
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)192
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()388879
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const>)> const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)365
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()730
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)365
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()44782
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)> const&)376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()376
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)282
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()752
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)376
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()376
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)188
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const>)> const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()198
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)99
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()364
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()19921
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)> const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()219
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)313
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()532
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)219
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::speed_tracker::SpeedTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*)94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()68533
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)407
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()814
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)407
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()693451
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const>)> const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)407
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()814
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)407
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()188
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)94
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()448
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()457
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()2151
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()282
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()564
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)282
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()224352
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)> const&)290
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()738
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<Tester>(mrs_lib::SubscribeHandlerOptions const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)95
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)100
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::uav_manager::UavManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)94
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfSource>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)100
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()1028
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)290
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const>)> const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)188
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()376
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)188
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2428
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const9942
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const17559
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const16455
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const379107
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const907275
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const116
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const185
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const18685
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const166115
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::lastMsgTime() const410827
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const107071
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const517469
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const66652
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const695
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::lastMsgTime() const260
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2492
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const18711
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::topicName[abi:cxx11]() const99
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const95
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const70911
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const18482
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const143367
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const104
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const90
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const108
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::lastMsgTime() const207966
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const242295
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const529124
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const36861
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const52850
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const91
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<2>*)::{lambda()#1}::operator()() const95
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const150
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const95
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const270
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const98
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const365
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const282
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const188
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const99
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const313
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::speed_tracker::SpeedTracker::*)(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>), mrs_uav_trackers::speed_tracker::SpeedTracker*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const407
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const407
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const95
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)::{lambda()#1}::operator()() const94
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfSource::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfSource*)::{lambda()#1}::operator()() const100
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const188
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.frameset.html new file mode 100644 index 0000000000..9acc9fca74 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html new file mode 100644 index 0000000000..e6862039b1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-04-25 05:42:57Functions:39074152.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines SubscribeHandler and related convenience classes for subscribing to ROS topics.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef SUBRSCRIBE_HANDLER_H
+       8             : #define SUBRSCRIBE_HANDLER_H
+       9             : 
+      10             : #include <ros/ros.h>
+      11             : #include <mrs_lib/timeout_manager.h>
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15             : 
+      16             :   static const ros::Duration no_timeout = ros::Duration(0);
+      17             : 
+      18             :   /* struct SubscribeHandlerOptions //{ */
+      19             :   
+      20             :   /**
+      21             :   * \brief A helper class to simplify setup of SubscribeHandler construction.
+      22             :   * This class is passed to the SubscribeHandler constructor and specifies its common options.
+      23             :   *
+      24             :   * \note Any option, passed directly to the SubscribeHandler constructor outside this structure, *OVERRIDES* values in this structure.
+      25             :   * The values in this structure can be thought of as default common values for all SubscribeHandler objects you want to create,
+      26             :   * and values passed directly to the constructor as specific options for the concrete handler.
+      27             :   *
+      28             :   */
+      29             :   struct SubscribeHandlerOptions
+      30             :   {
+      31             :     SubscribeHandlerOptions(const ros::NodeHandle& nh) : nh(nh) {}
+      32        2428 :     SubscribeHandlerOptions() = default;
+      33             :   
+      34             :     ros::NodeHandle nh;  /*!< \brief The ROS NodeHandle to be used for subscription. */
+      35             :   
+      36             :     std::string node_name = {};  /*!< \brief Name of the ROS node, using this handle (used for messages printed to console). */
+      37             :   
+      38             :     std::string topic_name = {};  /*!< \brief Name of the ROS topic to be handled. */
+      39             : 
+      40             :     std::shared_ptr<mrs_lib::TimeoutManager> timeout_manager = nullptr;  /*!< \brief Will be used for handling message timouts if necessary. If no manager is specified, it will be created with rate equal to half of \p no_message_timeout. */
+      41             :   
+      42             :     ros::Duration no_message_timeout = mrs_lib::no_timeout;  /*!< \brief If no new message is received for this duration, the \p timeout_callback function will be called. If \p timeout_callback is empty, an error message will be printed to the console. */
+      43             :   
+      44             :     std::function<void(const std::string& topic_name, const ros::Time& last_msg)> timeout_callback = {};  /*!< \brief This function will be called if no new message is received for the \p no_message_timeout duration. If this variable is empty, an error message will be printed to the console. */
+      45             :   
+      46             :     bool threadsafe = true;  /*!< \brief If true, all methods of the SubscribeHandler will be mutexed (using a recursive mutex) to avoid data races. */
+      47             :   
+      48             :     bool autostart = true;  /*!< \brief If true, the SubscribeHandler will be started after construction. Otherwise it has to be started using the start() method */
+      49             :   
+      50             :     uint32_t queue_size = 3;  /*!< \brief This parameter is passed to the NodeHandle when subscribing to the topic */
+      51             :   
+      52             :     ros::TransportHints transport_hints = ros::TransportHints();  /*!< \brief This parameter is passed to the NodeHandle when subscribing to the topic */
+      53             :   };
+      54             :   
+      55             :   //}
+      56             : 
+      57             :   /* SubscribeHandler class //{ */
+      58             :   /**
+      59             :   * \brief The main class for ROS topic subscription, message timeout handling etc.
+      60             :   *
+      61             :   * This class handles the raw ROS Subscriber for a specified topic. The last message received on the topic is remembered
+      62             :   * and may be retrieved by calling the getMsg() method. To check whether at least one message was received, use hasMsg()
+      63             :   * (if no message was received and you call getMsg(), a nullptr is returned and an error message is printed). To check
+      64             :   * whether a new message has arrived since the last call to getMsg(), use newMsg() (useful to check whether a new message
+      65             :   * needs to be processed in a loop or ROS Timer callback).
+      66             :   *
+      67             :   * A timeout callback function may be specified, which is called if no message is received on the topic for a specified
+      68             :   * timeout (use the \p timeout_callback and \p no_message_timeout parameters). If the timeout callback is not set by the
+      69             :   * user, an error message is printed to the console after the timeout by default.
+      70             :   *
+      71             :   * A message callback function may be specified, which is called whenever a new message is received (use the
+      72             :   * \p message_callback parameter).
+      73             :   *
+      74             :   * The callbacks and timeouts may be stopped and started using the stop() and start() methods.
+      75             :   *
+      76             :   * For more details, see the example below (I recommend starting with the simple_example which covers most use-cases).
+      77             :   *
+      78             :   */
+      79             :   template <typename MessageType>
+      80             :   class SubscribeHandler
+      81             :   {
+      82             :     public:
+      83             :     /*!
+      84             :       * \brief Convenience type for the template parameter to enable nice aliasing.
+      85             :       */
+      86             :       using message_type = MessageType;
+      87             : 
+      88             :     /*!
+      89             :       * \brief Type for the timeout callback function.
+      90             :       */
+      91             :       using timeout_callback_t = std::function<void(const std::string& topic_name, const ros::Time& last_msg)>;
+      92             : 
+      93             :     /*!
+      94             :       * \brief Convenience type for the message callback function.
+      95             :       */
+      96             :       using message_callback_t = std::function<void(typename MessageType::ConstPtr)>;
+      97             : 
+      98             :     public:
+      99             :     /*!
+     100             :       * \brief Returns the last received message on the topic, handled by this SubscribeHandler.
+     101             :       * Use hasMsg() first to check if at least one message is available or newMsg() to check if a new message
+     102             :       * since the last call to getMsg() is available.
+     103             :       *
+     104             :       * \return the last received message.
+     105             :       */
+     106     2567098 :       virtual typename MessageType::ConstPtr getMsg() {assert(m_pimpl); return m_pimpl->getMsg();};
+     107             : 
+     108             :     /*!
+     109             :       * \brief Returns the last received message on the topic without modifying the newMsg() or usedMsg() flags.
+     110             :       *
+     111             :       * \return the last received message.
+     112             :       */
+     113           0 :       virtual typename MessageType::ConstPtr peekMsg() const {assert(m_pimpl); return m_pimpl->peekMsg();};
+     114             : 
+     115             :     /*!
+     116             :       * \brief Used to check whether at least one message has been received on the handled topic.
+     117             :       *
+     118             :       * \return true if at least one message was received, otherwise false.
+     119             :       */
+     120     2768013 :       virtual bool hasMsg() const {assert(m_pimpl); return m_pimpl->hasMsg();};
+     121             : 
+     122             :     /*!
+     123             :       * \brief Used to check whether at least one message has been received on the handled topic since the last call to getMsg().
+     124             :       *
+     125             :       * \return true if at least one message was received, otherwise false.
+     126             :       */
+     127      517469 :       virtual bool newMsg() const {assert(m_pimpl); return m_pimpl->newMsg();};
+     128             : 
+     129             :     /*!
+     130             :       * \brief Used to check whether getMsg() was called at least once on this SubscribeHandler.
+     131             :       *
+     132             :       * \return true if getMsg() was called at least once, otherwise false.
+     133             :       */
+     134           0 :       virtual bool usedMsg() const {assert(m_pimpl); return m_pimpl->usedMsg();};
+     135             : 
+     136             :     /*!
+     137             :       * \brief Blocks until new data becomes available or until the timeout runs out or until a spurious wake-up.
+     138             :       *
+     139             :       * \return the message if a new message is available after waking up, \p nullptr otherwise.
+     140             :       */
+     141           0 :       virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout) {assert(m_pimpl); return m_pimpl->waitForNew(timeout);};
+     142             : 
+     143             :     /*!
+     144             :       * \brief Returns time of the last received message on the topic, handled by this SubscribeHandler.
+     145             :       *
+     146             :       * \return time when the last message was received.
+     147             :       */
+     148      656284 :       virtual ros::Time lastMsgTime() const {assert(m_pimpl); return m_pimpl->lastMsgTime();};
+     149             : 
+     150             :     /*!
+     151             :       * \brief Returns the resolved (full) name of the topic, handled by this SubscribeHandler.
+     152             :       *
+     153             :       * \return name of the handled topic.
+     154             :       */
+     155         320 :       virtual std::string topicName() const {assert(m_pimpl); return m_pimpl->topicName();};
+     156             : 
+     157             :     /*!
+     158             :       * \brief Returns the subscribed (unresolved) name of the topic, handled by this SubscribeHandler.
+     159             :       *
+     160             :       * \return name of the handled topic.
+     161             :       */
+     162           0 :       virtual std::string subscribedTopicName() const {assert(m_pimpl); return m_pimpl->m_topic_name;};
+     163             : 
+     164             :     /*!
+     165             :       * \brief Enables the callbacks for the handled topic.
+     166             :       *
+     167             :       * If the SubscribeHandler object is stopped using the stop() method, no callbacks will be called
+     168             :       * until the start() method is called.
+     169             :       */
+     170        6188 :       virtual void start() {assert(m_pimpl); return m_pimpl->start();};
+     171             : 
+     172             :     /*!
+     173             :       * \brief Disables the callbacks for the handled topic.
+     174             :       *
+     175             :       * All messages after this method is called will be ignored until start() is called again.
+     176             :       * Timeout checking will also be disabled.
+     177             :       */
+     178           5 :       virtual void stop() {assert(m_pimpl); return m_pimpl->stop();};
+     179             : 
+     180             :     public:
+     181             :     /*!
+     182             :       * \brief Default constructor to avoid having to use pointers.
+     183             :       *
+     184             :       * It does nothing and the SubscribeHandler it constructs will also do nothing.
+     185             :       * Use some of the other constructors for actual construction of a usable SubscribeHandler.
+     186             :       */
+     187        9447 :       SubscribeHandler() {};
+     188             : 
+     189             :     /*!
+     190             :       * \brief Main constructor.
+     191             :       *
+     192             :       * \param options    The common options struct (see documentation of SubscribeHandlerOptions).
+     193             :       * \param topic_name Name of the topic to be handled by this subscribed.
+     194             :       * \param args       Remaining arguments to be parsed (see other constructors).
+     195             :       *
+     196             :       */
+     197             :       template<class ... Types>
+     198        6184 :       SubscribeHandler(
+     199             :             const SubscribeHandlerOptions& options,
+     200             :             const std::string& topic_name,
+     201             :             Types ... args
+     202             :           )
+     203             :       :
+     204             :         SubscribeHandler(
+     205        6184 :             [options, topic_name]()
+     206             :             {
+     207       12368 :               SubscribeHandlerOptions opts = options;
+     208        6184 :               opts.topic_name = topic_name;
+     209       12368 :               return opts;
+     210             :             }(),
+     211             :             args...
+     212        6184 :             )
+     213             :       {
+     214        6184 :       }
+     215             : 
+     216             :     /*!
+     217             :       * \brief Convenience constructor overload.
+     218             :       *
+     219             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     220             :       * \param message_callback The callback function to call when a new message is received (you can leave this argument empty and just use the newMsg()/hasMsg() and getMsg() interface).
+     221             :       *
+     222             :       */
+     223        6184 :       SubscribeHandler(
+     224             :             const SubscribeHandlerOptions& options,
+     225             :             const message_callback_t& message_callback = {}
+     226             :           )
+     227        6184 :       {
+     228        6184 :         if (options.threadsafe)
+     229             :         {
+     230        6184 :           m_pimpl = std::make_unique<ImplThreadsafe>
+     231             :             (
+     232             :               options,
+     233             :               message_callback
+     234             :             );
+     235             :         }
+     236             :         else
+     237             :         {
+     238           0 :           m_pimpl = std::make_unique<Impl>
+     239             :             (
+     240             :               options,
+     241             :               message_callback
+     242             :             );
+     243             :         }
+     244        6184 :         if (options.autostart)
+     245        6183 :           start();
+     246        6184 :       };
+     247             : 
+     248             :     /*!
+     249             :       * \brief Convenience constructor overload.
+     250             :       *
+     251             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     252             :       * \param timeout_callback The callback function to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     253             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     254             :       *
+     255             :       */
+     256             :       template <class ... Types>
+     257           1 :       SubscribeHandler(
+     258             :             const SubscribeHandlerOptions& options,
+     259             :             const timeout_callback_t& timeout_callback,
+     260             :             Types ... args
+     261             :           )
+     262             :       :
+     263             :         SubscribeHandler(
+     264           1 :             [options, timeout_callback]()
+     265             :             {
+     266           2 :               SubscribeHandlerOptions opts = options;
+     267           1 :               opts.timeout_callback = timeout_callback;
+     268           2 :               return opts;
+     269             :             }(),
+     270             :             args...
+     271           1 :             )
+     272             :       {
+     273           1 :       }
+     274             : 
+     275             :     /*!
+     276             :       * \brief Convenience constructor overload.
+     277             :       *
+     278             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     279             :       * \param timeout_callback The callback method to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     280             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     281             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     282             :       *
+     283             :       */
+     284             :       template <class ObjectType1, class ... Types>
+     285             :       SubscribeHandler(
+     286             :             const SubscribeHandlerOptions& options,
+     287             :             void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     288             :             ObjectType1* const obj1,
+     289             :             Types ... args
+     290             :           )
+     291             :       :
+     292             :         SubscribeHandler(
+     293             :             [options, timeout_callback, obj1]()
+     294             :             {
+     295             :               SubscribeHandlerOptions opts = options;
+     296             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     297             :               return opts;
+     298             :             }(),
+     299             :             args...
+     300             :             )
+     301             :       {
+     302             :       }
+     303             : 
+     304             :     /*!
+     305             :       * \brief Convenience constructor overload.
+     306             :       *
+     307             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     308             :       * \param message_callback The callback method to call when a new message is received.
+     309             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     310             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     311             :       *
+     312             :       */
+     313             :       template <class ObjectType2, class ... Types>
+     314        2621 :       SubscribeHandler(
+     315             :             const SubscribeHandlerOptions& options,
+     316             :             void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     317             :             ObjectType2* const obj2,
+     318             :             Types ... args
+     319             :           )
+     320             :       :
+     321             :         SubscribeHandler(
+     322             :             options,
+     323        5242 :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     324             :             args...
+     325        2621 :             )
+     326             :       {
+     327        2621 :       }
+     328             : 
+     329             :     /*!
+     330             :       * \brief Convenience constructor overload.
+     331             :       *
+     332             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     333             :       * \param message_callback The callback method to call when a new message is received.
+     334             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     335             :       * \param timeout_callback The callback method to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     336             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     337             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     338             :       *
+     339             :       */
+     340             :      template <class ObjectType1, class ObjectType2, class ... Types>
+     341             :      SubscribeHandler(
+     342             :            const SubscribeHandlerOptions& options,
+     343             :            void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     344             :            ObjectType2* const obj2,
+     345             :            void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     346             :            ObjectType1* const obj1,
+     347             :            Types ... args
+     348             :          )
+     349             :      :
+     350             :        SubscribeHandler(
+     351             :             [options, timeout_callback, obj1]()
+     352             :             {
+     353             :               SubscribeHandlerOptions opts = options;
+     354             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     355             :               return opts;
+     356             :             }(),
+     357             :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     358             :             args...
+     359             :             )
+     360             :      {
+     361             :      }
+     362             : 
+     363             :     /*!
+     364             :       * \brief Convenience constructor overload.
+     365             :       *
+     366             :       * \param options            The common options struct (see documentation of SubscribeHandlerOptions).
+     367             :       * \param no_message_timeout If no message is received for this duration, the timeout callback function is called. If no timeout callback is specified, an error message is printed to the console.
+     368             :       * \param args               Remaining arguments to be parsed (see other constructors).
+     369             :       *
+     370             :       */
+     371             :       template<class ... Types>
+     372             :       SubscribeHandler(
+     373             :             const SubscribeHandlerOptions& options,
+     374             :             const ros::Duration& no_message_timeout,
+     375             :             Types ... args
+     376             :           )
+     377             :       :
+     378             :         SubscribeHandler(
+     379             :             [options, no_message_timeout]()
+     380             :             {
+     381             :               SubscribeHandlerOptions opts = options;
+     382             :               opts.no_message_timeout = no_message_timeout;
+     383             :               return opts;
+     384             :             }(),
+     385             :             args...
+     386             :             )
+     387             :       {
+     388             :       }
+     389             : 
+     390             :     /*!
+     391             :       * \brief Convenience constructor overload.
+     392             :       *
+     393             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     394             :       * \param timeout_manager  The manager for timeout callbacks.
+     395             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     396             :       *
+     397             :       */
+     398             :       template <class ... Types>
+     399             :       SubscribeHandler(
+     400             :             const SubscribeHandlerOptions& options,
+     401             :             mrs_lib::TimeoutManager& timeout_manager,
+     402             :             Types ... args
+     403             :           )
+     404             :       :
+     405             :         SubscribeHandler(
+     406             :             options,
+     407             :             timeout_manager = timeout_manager,
+     408             :             args...
+     409             :             )
+     410             :       {
+     411             :       }
+     412             : 
+     413       15651 :       ~SubscribeHandler() = default;
+     414             :       // delete copy constructor and assignment operator (forbid copying shandlers)
+     415             :       SubscribeHandler(const SubscribeHandler&) = delete;
+     416             :       SubscribeHandler& operator=(const SubscribeHandler&) = delete;
+     417             :       // define only move constructor and assignemnt operator
+     418          20 :       SubscribeHandler(SubscribeHandler&& other)
+     419          20 :       {
+     420          20 :         this->m_pimpl = std::move(other.m_pimpl);
+     421          20 :         other.m_pimpl = nullptr;
+     422          20 :       }
+     423        6070 :       SubscribeHandler& operator=(SubscribeHandler&& other)
+     424             :       {
+     425        6070 :         this->m_pimpl = std::move(other.m_pimpl);
+     426        6070 :         other.m_pimpl = nullptr;
+     427        6070 :         return *this;
+     428             :       }
+     429             : 
+     430             :     private:
+     431             :       class Impl;
+     432             :       class ImplThreadsafe;
+     433             :       std::unique_ptr<Impl> m_pimpl;
+     434             :   };
+     435             :   //}
+     436             : 
+     437             : /*!
+     438             :   * \brief Helper alias for convenient extraction of handled message type from a SubscribeHandlerPtr.
+     439             :   */
+     440             :   template<typename SubscribeHandler>
+     441             :   using message_type = typename SubscribeHandler::message_type;
+     442             : 
+     443             : /*!
+     444             :   * \brief Helper function for object construstion e.g. in case of member objects.
+     445             :   * This function is useful to avoid specifying object template parameters twice - once in definition of the variable and second time during object construction.
+     446             :   * This function can deduce the template parameters from the type of the already defined object, because it returns the newly constructed object as a reference
+     447             :   * argument and not as a return type.
+     448             :   *
+     449             :   * \param object The object to be constructed.
+     450             :   * \param args   These arguments are passed to the object constructor.
+     451             :   */
+     452             :   template<typename Class, class ... Types>
+     453             :   void construct_object(
+     454             :         Class& object,
+     455             :         Types ... args
+     456             :       )
+     457             :   {
+     458             :     object = Class(args...);
+     459             :   }
+     460             : }
+     461             : 
+     462             : #include <mrs_lib/impl/subscribe_handler.hpp>
+     463             : 
+     464             : #endif // SUBRSCRIBE_HANDLER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html new file mode 100644 index 0000000000..056386c498 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html @@ -0,0 +1,136 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b6e9ccb1d3175600727e604938e2df9a1db1b393 GIT binary patch literal 1783 zcmVGAf72gj3K5r zbz2ND5}}4FQEmJ&0qHg3YyO-C@r%I16(bOAcI{2~uIrUq&|?~*Fbke#KB6~6I8Vnr z*#`C}y~&UtGp8aqYx()_!9H3$vYLf#J|t)Of*?k(LiRez2b~a-7VDb>@sJ(OR zSPm&vvJ`~KR`Ok-ZVqcFgM8B6WXF=d6m!tz668ytM&>Z-6K5u6q!$AOc%jcH>3fS({;hPmwMn+C2MvDs~C)tCPOw1-3bupJYJgBin5(kdU98OtK zN-*pu2VTAz1vBYHiE0uiLlN>kL#(mB6FXs73unw;3sz_yc$`3{Fzh2wZJ5Y%O{q?O z6g)UZIm3|l&g)}FbTz%1oSPcWrg26f^ROQycTEwZjt5oF07LA8$5R1_ppf;LRN)NXH-nI=1IRZ;*ZXa1Jm$BBU5$hac6W< z>vHD0c*JRm88FCYuB0SXfeBrz8I(kg#WX;~w>RzSqhg>oXf`nCRNAM4{w5XvjlTQ9 zi<_&mKLc=%xTNe^@O*xL-tYJCpTGOpo!b|_@K;}}ps7tqC`HZ1s>Z;0N`fAZxipr! zT+JxJq3Mhs445)zWWji4^t|CQ!b8ANjVo;ChlhY6?T6suCbcma-FuA(#UuKr*Q4ycwyjT-@Af7jjOV# z1}X6BnolZtX|LeqUB29-p!+za)O=u=YT11ec{{UJ2gnCyJ$DS0)QGKqEA> zyv^&elvmKyrlVVS{8|Sfw;woTVV40RMpHY^Ip6CV;4}VE?-si#x8M(3=8X zL5s=>ou>R#tqPAJ6&N4mRxM#Slrw2_gpYNmF=oCUVf0`e;s!TL)>@Lh6M z1b;Q9l#Bu*NR%Sm|N54f;T}SbW~y~$N6c$RxX$#k;(w-iXq@Ae&+a*xjc`l|iCimSGlp%iNo zmj%`H0CA`Bg?F!~%P6IkY8v=0l;Y7Tx!)1|3+Xzm-Sze((0I;u;~&MXuQxstxT(KY zX(e(pqT9~%KW^TUqLm<}a6q|Gi{807Q{a<@olDO1>&?kSZ5w1K*wbh>t$iprRF+ck ziO$Ts4mAXAeRMQ)PEX@CnHIEgiUfFjEjoFq{_I)!@FN41v;5;w+=cL$!u>PPEJ%xJ zh?{xIOs}QF171_jPJ9*c4`*F09O$uQMu)yez?5JhmHe;jSDyG-brQgzcQ(Vv?+oq; zK?^@BX1I#M8`Opk0%sintuOW+}mrx)Rs|8>y8oYETAgY|8< Z{{wkhUB=a#p|$`3002ovPDHLkV1nx6SeyU= literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html b/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html new file mode 100644 index 0000000000..160ba65b3b --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.func.html b/mrs_lib/include/mrs_lib/timeout_manager.h.func.html new file mode 100644 index 0000000000..ff3d2d7d16 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.func.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.frameset.html new file mode 100644 index 0000000000..e77822ab61 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html new file mode 100644 index 0000000000..edacfbf89d --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html @@ -0,0 +1,169 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:00-
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief TODO
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef TIMEOUT_MANAGER_H
+       8             : #define TIMEOUT_MANAGER_H
+       9             : 
+      10             : #include <ros/ros.h>
+      11             : 
+      12             : namespace mrs_lib
+      13             : {
+      14             :   /* TimeoutManager class //{ */
+      15             :   /**
+      16             :   * \brief TODO
+      17             :   *
+      18             :   */
+      19             :   class TimeoutManager
+      20             :   {
+      21             :     public:
+      22             :       // | ---------------------- public types ---------------------- |
+      23             :       using timeout_id_t = size_t;
+      24             :       using callback_t = std::function<void(const ros::Time&)>;
+      25             : 
+      26             :     public:
+      27             :       // | --------------------- public methods --------------------- |
+      28             : 
+      29             :     /*!
+      30             :       * \brief TODO
+      31             :       *
+      32             :       */
+      33             :       TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate);
+      34             : 
+      35          78 :       timeout_id_t registerNew(const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset = ros::Time::now(), const bool oneshot = false, const bool autostart = true);
+      36             : 
+      37      149668 :       void reset(const timeout_id_t id, const ros::Time& time = ros::Time::now());
+      38             : 
+      39             :       void pause(const timeout_id_t id);
+      40             : 
+      41          82 :       void start(const timeout_id_t id, const ros::Time& time = ros::Time::now());
+      42             : 
+      43             :       void pauseAll();
+      44             : 
+      45           2 :       void startAll(const ros::Time& time = ros::Time::now());
+      46             : 
+      47             :       void change(const timeout_id_t id, const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset = ros::Time::now(), const bool oneshot = false, const bool autostart = true);
+      48             : 
+      49             :       ros::Time lastReset(const timeout_id_t id);
+      50             : 
+      51             :       bool started(const timeout_id_t id);
+      52             : 
+      53             :       /* implementation details //{ */
+      54             :       
+      55             :       private:
+      56             :         // | ---------------------- private types --------------------- |
+      57             :         struct timeout_info_t
+      58             :         {
+      59             :           bool oneshot;
+      60             :           bool started;
+      61             :           callback_t callback;
+      62             :           ros::Duration timeout;
+      63             :           ros::Time last_reset;
+      64             :           ros::Time last_callback;
+      65             :         };
+      66             :       
+      67             :       private:
+      68             :         // | --------------------- private methods -------------------- |
+      69             :         void main_timer_callback([[maybe_unused]] const ros::TimerEvent& evt);
+      70             :       
+      71             :       private:
+      72             :         // | ------------------------- members ------------------------ |
+      73             :         std::recursive_mutex m_mtx;
+      74             :         timeout_id_t m_last_id;
+      75             :         std::vector<timeout_info_t> m_timeouts;
+      76             :       
+      77             :         ros::Timer m_main_timer;
+      78             :       
+      79             :       //}
+      80             : 
+      81             :   };
+      82             :   //}
+      83             : }
+      84             : 
+      85             : #endif // TIMEOUT_MANAGER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html new file mode 100644 index 0000000000..bc8b5520c4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html @@ -0,0 +1,42 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.png b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8fad76eb55ea38224226aeafae950c60603e0c51 GIT binary patch literal 435 zcmV;k0ZjghP)FbGV30AJ7_B>(?gH76boi%nZP&RAz%mP1g-o|!nA8yb_>Ak{J@2IPED z(QvE76Xq8;Zb5G+Q-BuNDP}a87$YJ2@BoU!otC!W0Elu@E;9fG2_2ljUcy zeedj?1eRACO;ed}82YA;lgv+B^kFb%aEJ$&%+KeDnOkjZCRF>Y>n}7 z63Ti0yY4{9GUF;Ti4T5V#Rb56dy_r(|@l~>CSqvT + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::MRSTimer::~MRSTimer()0
mrs_lib::ThreadTimer::~ThreadTimer()8
mrs_lib::ThreadTimer::~ThreadTimer().28
mrs_lib::ROSTimer::~ROSTimer()8
mrs_lib::ROSTimer::~ROSTimer().28
mrs_lib::MRSTimer::MRSTimer()16
mrs_lib::MRSTimer::~MRSTimer().216
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/timer.h.func.html b/mrs_lib/include/mrs_lib/timer.h.func.html new file mode 100644 index 0000000000..a3ff4af2b7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.func.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::~ThreadTimer()8
mrs_lib::ThreadTimer::~ThreadTimer().28
mrs_lib::MRSTimer::MRSTimer()16
mrs_lib::MRSTimer::~MRSTimer()0
mrs_lib::MRSTimer::~MRSTimer().216
mrs_lib::ROSTimer::~ROSTimer()8
mrs_lib::ROSTimer::~ROSTimer().28
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/timer.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/timer.h.gcov.frameset.html new file mode 100644 index 0000000000..558a47b8e4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/timer.h.gcov.html b/mrs_lib/include/mrs_lib/timer.h.gcov.html new file mode 100644 index 0000000000..74935862f3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.gcov.html @@ -0,0 +1,355 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_TIMER_H
+       2             : #define MRS_TIMER_H
+       3             : 
+       4             : #include <ros/ros.h>
+       5             : #include <thread>
+       6             : #include <mutex>
+       7             : #include <condition_variable>
+       8             : #include <atomic>
+       9             : 
+      10             : namespace mrs_lib
+      11             : {
+      12             :   bool breakable_sleep(const ros::Duration& dur, const std::atomic<bool>& continue_sleep);
+      13             : 
+      14             :   /**
+      15             :    * @brief Common wrapper representing the functionality of the ros::Timer.
+      16             :    *
+      17             :    * The implementation can then use either ros::Timer (the ROSTimer class)
+      18             :    * or threads and synchronization primitives from the C++ standard library
+      19             :    * (the ThreadTimer class). Both these variants implement the same interface.
+      20             :    *
+      21             :    * @note Functionality of the two implementations differs in some details.
+      22             :    */
+      23             :   class MRSTimer
+      24             :   {
+      25             :     public:
+      26             :     using callback_t = std::function<void(const ros::TimerEvent&)>;
+      27             : 
+      28             :     /**
+      29             :      * @brief stop the timer
+      30             :      */
+      31             :     virtual void stop() = 0;
+      32             : 
+      33             :     /**
+      34             :      * @brief start the timer
+      35             :      */
+      36             :     virtual void start() = 0;
+      37             : 
+      38             :     /**
+      39             :      * @brief set the timer period/duration
+      40             :      *
+      41             :      * @param duration
+      42             :      * @param reset
+      43             :      */
+      44             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) = 0;
+      45             : 
+      46             :     /**
+      47             :      * @brief change the callback method 
+      48             :      *
+      49             :      * Usable e.g. for running thread with a specific parameter if you bind it using std::bind
+      50             :      *
+      51             :      * @param callback          callback method to be called.
+      52             :      */
+      53             :     virtual void setCallback(const std::function<void(const ros::TimerEvent&)>& callback) = 0;
+      54             : 
+      55             :     /**
+      56             :      * @brief returns true if callbacks should be called
+      57             :      *
+      58             :      * @return true if timer is running
+      59             :      */
+      60             :     virtual bool running() = 0;
+      61             : 
+      62          16 :     virtual ~MRSTimer() = default;
+      63             :     MRSTimer(const MRSTimer&) = default;
+      64             :     MRSTimer(MRSTimer&&) = default;
+      65             :     MRSTimer& operator=(const MRSTimer&) = default;
+      66             :     MRSTimer& operator=(MRSTimer&&) = default;
+      67             : 
+      68             :     protected:
+      69          16 :     MRSTimer() = default;
+      70             :   };
+      71             : 
+      72             :   // | ------------------------ ROSTimer ------------------------ |
+      73             : 
+      74             :   /* class ROSTimer //{ */
+      75             : 
+      76             :   /**
+      77             :    * @brief ros::Timer wrapper. The interface is the same as with ros::Timer, except for the initialization method.
+      78             :    */
+      79             :   class ROSTimer : public MRSTimer {
+      80             :   public:
+      81             :     ROSTimer();
+      82             : 
+      83             :     /**
+      84             :      * @brief Constructs the object.
+      85             :      *
+      86             :      * @tparam ObjectType
+      87             :      * @param nh                            ROS node handle to be used for creating the underlying ros::Timer object.
+      88             :      * @param rate                          rate at which the callback should be called.
+      89             :      * @param ObjectType::*const callback   callback method to be called.
+      90             :      * @param obj                           object for the method.
+      91             :      * @param oneshot                       whether the callback should only be called once after starting.
+      92             :      * @param autostart                     whether the timer should immediately start after construction.
+      93             :      */
+      94             :     template <class ObjectType>
+      95             :     ROSTimer(const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+      96             :              const bool oneshot = false, const bool autostart = true);
+      97             : 
+      98             :     /**
+      99             :      * @brief full constructor
+     100             :      *
+     101             :      * @tparam ObjectType
+     102             :      * @param nh                            ROS node handle to be used for creating the underlying ros::Timer object.
+     103             :      * @param duration                      desired callback period.
+     104             :      * @param ObjectType::*const callback   callback method to be called.
+     105             :      * @param obj                           object for the method.
+     106             :      * @param oneshot                       whether the callback should only be called once after starting.
+     107             :      * @param autostart                     whether the timer should immediately start after construction.
+     108             :      */
+     109             :     template <class ObjectType>
+     110             :     ROSTimer(const ros::NodeHandle& nh, const ros::Duration& duration, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     111             :              const bool oneshot = false, const bool autostart = true);
+     112             : 
+     113             :     /**
+     114             :      * @brief stop the timer
+     115             :      */
+     116             :     virtual void stop() override;
+     117             : 
+     118             :     /**
+     119             :      * @brief start the timer
+     120             :      */
+     121             :     virtual void start() override;
+     122             : 
+     123             :     /**
+     124             :      * @brief set the timer period/duration
+     125             :      *
+     126             :      * @param duration
+     127             :      * @param reset
+     128             :      */
+     129             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) override;
+     130             : 
+     131             :     /**
+     132             :      * @brief change the callback method
+     133             :      *
+     134             :      * Usable e.g. for running thread with a specific parameter if you bind it using std::bind
+     135             :      *
+     136             :      * @param callback          callback method to be called.
+     137             :      */
+     138             :     virtual void setCallback(const std::function<void(const ros::TimerEvent&)>& callback) override;
+     139             : 
+     140             :     /**
+     141             :      * @brief returns true if callbacks should be called
+     142             :      *
+     143             :      * @return true if timer is running
+     144             :      */
+     145             :     virtual bool running() override;
+     146             : 
+     147          16 :     virtual ~ROSTimer() override {stop();};
+     148             :     // to keep rule of five since we have a custom destructor
+     149             :     ROSTimer(const ROSTimer&) = delete;
+     150             :     ROSTimer(ROSTimer&&) = delete;
+     151             :     ROSTimer& operator=(const ROSTimer&) = delete;
+     152             :     ROSTimer& operator=(ROSTimer&&) = delete;
+     153             : 
+     154             :   private:
+     155             :     std::mutex mutex_timer_;
+     156             : 
+     157             :     std::unique_ptr<ros::Timer> timer_;
+     158             :   };
+     159             : 
+     160             :   //}
+     161             : 
+     162             :   // | ----------------------- ThreadTimer ---------------------- |
+     163             : 
+     164             :   /* class ThreadTimer //{ */
+     165             : 
+     166             :   /**
+     167             :    * @brief Custom thread-based Timers with the same interface as mrs_lib::ROSTimer.
+     168             :    */
+     169             :   class ThreadTimer : public MRSTimer {
+     170             : 
+     171             :   public:
+     172             :     ThreadTimer();
+     173             : 
+     174             :     /**
+     175             :      * @brief Constructs the object.
+     176             :      *
+     177             :      * @tparam ObjectType
+     178             :      * @param nh                            ignored (used just for consistency with ROSTimer)
+     179             :      * @param rate                          rate at which the callback should be called.
+     180             :      * @param ObjectType::*const callback   callback method to be called.
+     181             :      * @param obj                           object for the method.
+     182             :      * @param oneshot                       whether the callback should only be called once after starting.
+     183             :      * @param autostart                     whether the timer should immediately start after construction.
+     184             :      */
+     185             :     template <class ObjectType>
+     186             :     ThreadTimer(const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     187             :                 const bool oneshot = false, const bool autostart = true);
+     188             : 
+     189             :     /**
+     190             :      * @brief Constructs the object.
+     191             :      *
+     192             :      * @tparam ObjectType
+     193             :      * @param nh                            ignored (used just for consistency with ROSTimer)
+     194             :      * @param duration                      desired callback period.
+     195             :      * @param ObjectType::*const callback   callback method to be called.
+     196             :      * @param obj                           object for the method.
+     197             :      * @param oneshot                       whether the callback should only be called once after starting.
+     198             :      * @param autostart                     whether the timer should immediately start after construction.
+     199             :      */
+     200             :     template <class ObjectType>
+     201             :     ThreadTimer(const ros::NodeHandle& nh, const ros::Duration& duration, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     202             :                 bool oneshot = false, const bool autostart = true);
+     203             : 
+     204             :     /**
+     205             :      * @brief stop the timer
+     206             :      *
+     207             :      * No more callbacks will be called after this method returns.
+     208             :      */
+     209             :     virtual void stop() override;
+     210             : 
+     211             :     /**
+     212             :      * @brief start the timer
+     213             :      *
+     214             :      * The first callback will be called in now + period.
+     215             :      *
+     216             :      * @note If the timer is already started, nothing will change.
+     217             :      */
+     218             :     virtual void start() override;
+     219             : 
+     220             :     /**
+     221             :      * @brief set the timer period/duration
+     222             :      *
+     223             :      * The new period will be applied after the next callback.
+     224             :      *
+     225             :      * @param duration the new desired callback period.
+     226             :      * @param reset    ignored in this implementation.
+     227             :      */
+     228             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) override;
+     229             :     
+     230             :     /**
+     231             :      * @brief change the callback method
+     232             :      *
+     233             :      * Usable e.g. for running thread with a specific parameter if you bind it using std::bind
+     234             :      *
+     235             :      * @param callback          callback method to be called.
+     236             :      */
+     237             :     virtual void setCallback(const std::function<void(const ros::TimerEvent&)>& callback) override;
+     238             : 
+     239             :     /**
+     240             :      * @brief returns true if callbacks should be called
+     241             :      *
+     242             :      * @return true if timer is running
+     243             :      */
+     244             :     virtual bool running() override;
+     245             : 
+     246             :     /**
+     247             :      * @brief stops the timer and then destroys the object
+     248             :      *
+     249             :      * No more callbacks will be called when the destructor is started.
+     250             :      */
+     251          16 :     virtual ~ThreadTimer() override {stop();};
+     252             :     // to keep rule of five since we have a custom destructor
+     253             :     ThreadTimer(const ThreadTimer&) = delete;
+     254             :     ThreadTimer(ThreadTimer&&) = delete;
+     255             :     ThreadTimer& operator=(const ThreadTimer&) = delete;
+     256             :     ThreadTimer& operator=(ThreadTimer&&) = delete;
+     257             : 
+     258             :   private:
+     259             :     class Impl;
+     260             : 
+     261             :     std::unique_ptr<Impl> impl_;
+     262             : 
+     263             :   };  // namespace mrs_lib
+     264             : 
+     265             :   //}
+     266             : 
+     267             : #include <mrs_lib/impl/timer.hpp>
+     268             : 
+     269             : }  // namespace mrs_lib
+     270             : 
+     271             : #endif  // MRS_TIMER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/timer.h.gcov.overview.html b/mrs_lib/include/mrs_lib/timer.h.gcov.overview.html new file mode 100644 index 0000000000..d88669cb74 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.gcov.overview.html @@ -0,0 +1,88 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/timer.h.gcov.png b/mrs_lib/include/mrs_lib/timer.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..7b39300b63d0b8b3c26f48a5ebff8d932b14161e GIT binary patch literal 935 zcmV;Y16cftP)B8%(T`(U zA#=WBMF1CF1_~tAoSLB3wB9qhID)EY5bF;NXw0rrjSX-(rUn5I6AFtPa11Gcg#HQe zE_f2C;RF*cH~`O-js1AMUa$4Ftu)Dn~K2rAD4UkSMwq7W+zZGcHiPu=c zrXffD+=5;!&?IxI9zbTy9P z)0{)RwWl9ZGF?8SPAFC_>antqrC^khwi%V6D&cs3Hsn@er!g8VLEzF{3TnF{GKWp$ zQIMvM6pW+d${^8tNrfA<$vie>Kqw%3gjfimbKJ_N-Cc99{C%Egg@3vL?t{5$#wet| zKyA`b)9eky*{mAeQ4Eq4`=sqR`c;5&Z-*UH=NDb*m^jV(d&FTd#>C6ukl~H ztj9N&^7z13)RJ0gHl(#Ft}WQJZd!JXHC?Y*+-cP0uS7D*oYpvXLWvrSNfY*a`@*Cd z>)8~4hkjR}JNZ@*L_q(f$vxBvAbX9jk#rVklg9sbB$ZP=GfRBj3Em^A5-h4N@W$j0 zNyQ{>DHHkDaikEC_M3eBV+2PHm + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-04-25 05:42:57Functions:141877.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
std::optional<boost::shared_ptr<geometry_msgs::Quaternion_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<geometry_msgs::Quaternion_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
std::optional<boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::Transformer::inverse(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)294
mrs_lib::Transformer::retryLookupNewest(bool)471
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1508
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)455603
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1934431
mrs_lib::Transformer::create_transform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, geometry_msgs::Transform_<std::allocator<void> > const&)1934488
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1937200
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1943518
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1943523
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/transformer.h.func.html b/mrs_lib/include/mrs_lib/transformer.h.func.html new file mode 100644 index 0000000000..17255e89d3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-04-25 05:42:57Functions:141877.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1943523
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1934431
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1508
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)455603
std::optional<boost::shared_ptr<geometry_msgs::Quaternion_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<geometry_msgs::Quaternion_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
std::optional<boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::create_transform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, geometry_msgs::Transform_<std::allocator<void> > const&)1934488
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)294
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::retryLookupNewest(bool)471
mrs_lib::Transformer::inverse(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1943518
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1937200
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/transformer.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/transformer.h.gcov.frameset.html new file mode 100644 index 0000000000..b78146f575 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/transformer.h.gcov.html b/mrs_lib/include/mrs_lib/transformer.h.gcov.html new file mode 100644 index 0000000000..6a5ce18df2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.gcov.html @@ -0,0 +1,764 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-04-25 05:42:57Functions:141877.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : 
+       3             : /**  \file Implements the Transformer class - wrapper for ROS's TF2 lookups and transformations.
+       4             :  *   \brief A wrapper for easier work with tf2 transformations.
+       5             :  *   \author Tomas Baca - tomas.baca@fel.cvut.cz
+       6             :  *   \author Matouš Vrba - matous.vrba@fel.cvut.cz
+       7             :  */
+       8             : #ifndef TRANSFORMER_H
+       9             : #define TRANSFORMER_H
+      10             : 
+      11             : /* #define EIGEN_DONT_VECTORIZE */
+      12             : /* #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT */
+      13             : 
+      14             : /* includes //{ */
+      15             : 
+      16             : #include <ros/ros.h>
+      17             : 
+      18             : #include <tf2_ros/transform_listener.h>
+      19             : #include <tf2_ros/buffer.h>
+      20             : #include <tf2_eigen/tf2_eigen.h>
+      21             : #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+      22             : #include <tf/transform_datatypes.h>
+      23             : #include <tf_conversions/tf_eigen.h>
+      24             : 
+      25             : #include <mrs_msgs/ReferenceStamped.h>
+      26             : 
+      27             : #include <geometry_msgs/PoseStamped.h>
+      28             : #include <geometry_msgs/Vector3Stamped.h>
+      29             : #include <std_msgs/Header.h>
+      30             : 
+      31             : #include <mrs_lib/geometry/misc.h>
+      32             : 
+      33             : #include <pcl_ros/point_cloud.h>
+      34             : #include <pcl_ros/transforms.h>
+      35             : #include <pcl_conversions/pcl_conversions.h>
+      36             : 
+      37             : #include <mutex>
+      38             : #include <experimental/type_traits>
+      39             : 
+      40             : //}
+      41             : 
+      42             : namespace tf2
+      43             : {
+      44             : 
+      45             :   template <typename pt_t>
+      46             :   void doTransform(const pcl::PointCloud<pt_t>& cloud_in, pcl::PointCloud<pt_t>& cloud_out, const geometry_msgs::TransformStamped& transform)
+      47             :   {
+      48             :     pcl_ros::transformPointCloud(cloud_in, cloud_out, transform.transform);
+      49             :     pcl_conversions::toPCL(transform.header.stamp, cloud_out.header.stamp);
+      50             :     cloud_out.header.frame_id = transform.header.frame_id;
+      51             :   }
+      52             : 
+      53             : }  // namespace tf2
+      54             : 
+      55             : namespace mrs_lib
+      56             : {
+      57             : 
+      58             :   static const std::string UTM_ORIGIN = "utm_origin";
+      59             :   static const std::string LATLON_ORIGIN = "latlon_origin";
+      60             : 
+      61             :   /**
+      62             :    * \brief A convenience wrapper class for ROS's native TF2 API to simplify transforming of various messages.
+      63             :    *
+      64             :    * Implements optional automatic frame prefix deduction, seamless transformation lattitude/longitude coordinates and UTM coordinates, simple transformation of MRS messages etc.
+      65             :    */
+      66             :   /* class Transformer //{ */
+      67             : 
+      68             :   class Transformer
+      69             :   {
+      70             : 
+      71             :   public:
+      72             :     /* Constructor, assignment operator and overloads //{ */
+      73             : 
+      74             :     /**
+      75             :      * \brief A convenience constructor that doesn't initialize anything.
+      76             :      *
+      77             :      * This constructor is just to enable usign the Transformer as a member variable of nodelets etc.
+      78             :      * To actually initialize the class, use the alternative constructor.
+      79             :      *
+      80             :      * \note This constructor doesn't initialize the TF2 transform listener and all calls to the transformation-related methods of an object constructed using this method will fail.
+      81             :      */
+      82             :     Transformer();
+      83             : 
+      84             :     /**
+      85             :      * \brief The main constructor that actually initializes stuff.
+      86             :      *
+      87             :      * This constructor initializes the class and the TF2 transform listener.
+      88             :      *
+      89             :      * \param node_name   the name of the node running the transformer, is used in ROS prints. If you don't care, just set it to an empty string.
+      90             :      * \param cache_time  duration of the transformation buffer's cache into the past that will be kept.
+      91             :      */
+      92             :     Transformer(const std::string& node_name, const ros::Duration& cache_time = ros::Duration(tf2_ros::Buffer::DEFAULT_CACHE_TIME));
+      93             : 
+      94             :     /**
+      95             :      * \brief The main constructor that actually initializes stuff.
+      96             :      *
+      97             :      * This constructor initializes the class and the TF2 transform listener.
+      98             :      *
+      99             :      * \param nh          the node handle to be used for subscribing to the transformations.
+     100             :      * \param node_name   the name of the node running the transformer, is used in ROS prints. If you don't care, just set it to an empty string.
+     101             :      * \param cache_time  duration of the transformation buffer's cache into the past that will be kept.
+     102             :      */
+     103             :     Transformer(const ros::NodeHandle& nh, const std::string& node_name = std::string(), const ros::Duration& cache_time = ros::Duration(tf2_ros::Buffer::DEFAULT_CACHE_TIME));
+     104             : 
+     105             :     /**
+     106             :      * \brief A convenience move assignment operator.
+     107             :      *
+     108             :      * This operator moves all data from the object that is being assigned from, invalidating it.
+     109             :      *
+     110             :      * \param  other  the object to assign from. It will be invalid after this method returns.
+     111             :      * \return        a reference to the object being assigned to.
+     112             :      */
+     113             :     Transformer& operator=(Transformer&& other);
+     114             : 
+     115             :     //}
+     116             : 
+     117             :     // | ------------------ Configuration methods ----------------- |
+     118             : 
+     119             :     /* setDefaultFrame() //{ */
+     120             : 
+     121             :     /**
+     122             :      * \brief Sets the default frame ID to be used instead of any empty frame ID.
+     123             :      *
+     124             :      * If you call e.g. the transform() method with a message that has an empty header.frame_id field, this value will be used instead.
+     125             :      *
+     126             :      * \param frame_id the default frame ID. Use an empty string to disable default frame ID deduction.
+     127             :      *
+     128             :      * \note Disabled by default.
+     129             :      */
+     130      455603 :     void setDefaultFrame(const std::string& frame_id)
+     131             :     {
+     132      910869 :       std::scoped_lock lck(mutex_);
+     133      455507 :       default_frame_id_ = frame_id;
+     134      455450 :     }
+     135             : 
+     136             :     //}
+     137             : 
+     138             :     /* setDefaultPrefix() //{ */
+     139             : 
+     140             :     /**
+     141             :      * \brief Sets the default frame ID prefix to be used if no prefix is present in the frame.
+     142             :      *
+     143             :      * If you call any method with a frame ID that doesn't begin with this string, it will be automatically prefixed including a forward slash between the prefix and raw frame ID.
+     144             :      * The forward slash should therefore *not* be included in the prefix.
+     145             :      *
+     146             :      * Example frame ID resolution assuming default prefix is "uav1":
+     147             :      *   "local_origin" -> "uav1/local_origin"
+     148             :      *
+     149             :      * \param prefix the default frame ID prefix (without the forward slash at the end). Use an empty string to disable default frame ID prefixing.
+     150             :      *
+     151             :      * \note Disabled by default. The prefix will be applied as a namespace (with a forward slash between the prefix and raw frame ID).
+     152             :      */
+     153         294 :     void setDefaultPrefix(const std::string& prefix)
+     154             :     {
+     155         588 :       std::scoped_lock lck(mutex_);
+     156         294 :       if (prefix.empty())
+     157           1 :         prefix_ = "";
+     158             :       else
+     159         293 :         prefix_ = prefix + "/";
+     160         294 :     }
+     161             : 
+     162             :     //}
+     163             : 
+     164             :     /* setLatLon() //{ */
+     165             : 
+     166             :     /**
+     167             :      * \brief Sets the curret lattitude and longitude for UTM zone calculation.
+     168             :      *
+     169             :      * The Transformer uses this to deduce the current UTM zone used for transforming stuff to latlon_origin.
+     170             :      *
+     171             :      * \param lat the latitude in degrees.
+     172             :      * \param lon the longitude in degrees.
+     173             :      *
+     174             :      * \note Any transformation to latlon_origin will fail if this function is not called first!
+     175             :      */
+     176             :     void setLatLon(const double lat, const double lon);
+     177             : 
+     178             :     //}
+     179             : 
+     180             :     /* setLookupTimeout() //{ */
+     181             : 
+     182             :     /**
+     183             :      * \brief Set a timeout for transform lookup.
+     184             :      *
+     185             :      * The transform lookup operation will block up to this duration if the transformation is not available immediately.
+     186             :      *
+     187             :      * \note Disabled by default.
+     188             :      *
+     189             :      * \param timeout the lookup timeout. Set to zero to disable blocking.
+     190             :      */
+     191           2 :     void setLookupTimeout(const ros::Duration timeout = ros::Duration(0))
+     192             :     {
+     193           2 :       std::scoped_lock lck(mutex_);
+     194           2 :       lookup_timeout_ = timeout;
+     195           2 :     }
+     196             : 
+     197             :     //}
+     198             : 
+     199             :     /* retryLookupNewest() //{ */
+     200             : 
+     201             :     /**
+     202             :      * \brief Enable/disable retry of a failed transform lookup with \p ros::Time(0).
+     203             :      *
+     204             :      * If enabled, a failed transform lookup will be retried.
+     205             :      * The new try will ignore the requested timestamp and will attempt to fetch the latest transformation between the two frames.
+     206             :      *
+     207             :      * \note Disabled by default.
+     208             :      *
+     209             :      * \param retry enables or disables retry.
+     210             :      */
+     211         471 :     void retryLookupNewest(const bool retry = true)
+     212             :     {
+     213         471 :       std::scoped_lock lck(mutex_);
+     214         471 :       retry_lookup_newest_ = retry;
+     215         471 :     }
+     216             : 
+     217             :     //}
+     218             :     
+     219             :     /* beQuiet() //{ */
+     220             : 
+     221             :     /**
+     222             :      * \brief Enable/disable some prints that may be too noisy.
+     223             :      *
+     224             :      * \param quiet enables or disables quiet mode.
+     225             :      *
+     226             :      * \note Disabled by default.
+     227             :      */
+     228             :     void beQuiet(const bool quiet = true)
+     229             :     {
+     230             :       std::scoped_lock lck(mutex_);
+     231             :       quiet_ = quiet;
+     232             :     }
+     233             : 
+     234             :     //}
+     235             : 
+     236             :     /* resolveFrame() //{ */
+     237             :     /**
+     238             :      * \brief Deduce the full frame ID from a shortened or empty string using current default prefix and default frame rules.
+     239             :      *
+     240             :      * Example assuming default prefix is "uav1" and default frame is "uav1/gps_origin":
+     241             :      *   "" -> "uav1/gps_origin"
+     242             :      *   "local_origin" -> "uav1/local_origin"
+     243             :      *
+     244             :      * \param frame_id The frame ID to be resolved.
+     245             :      *
+     246             :      * \return The resolved frame ID.
+     247             :      */
+     248        1508 :     std::string resolveFrame(const std::string& frame_id)
+     249             :     {
+     250        3016 :       std::scoped_lock lck(mutex_);
+     251        3016 :       return resolveFrameImpl(frame_id);
+     252             :     }
+     253             :     //}
+     254             : 
+     255             :     /* transformSingle() //{ */
+     256             : 
+     257             :     /**
+     258             :      * \brief Transforms a single variable to a new frame and returns it or \p std::nullopt if transformation fails.
+     259             :      *
+     260             :      * \param what the object to be transformed.
+     261             :      * \param to_frame the target frame ID.
+     262             :      *
+     263             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     264             :      */
+     265             :     template <class T>
+     266             :     [[nodiscard]] std::optional<T> transformSingle(const T& what, const std::string& to_frame);
+     267             : 
+     268             :     /**
+     269             :      * \brief Transforms a single variable to a new frame.
+     270             :      *
+     271             :      * A convenience override for shared pointers.
+     272             :      *
+     273             :      * \param what The object to be transformed.
+     274             :      * \param to_frame The target fram ID.
+     275             :      *
+     276             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     277             :      */
+     278             :     template <class T>
+     279           1 :     [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const boost::shared_ptr<T>& what, const std::string& to_frame)
+     280             :     {
+     281           1 :       return transformSingle(boost::shared_ptr<const T>(what), to_frame);
+     282             :     }
+     283             : 
+     284             :     /**
+     285             :      * \brief Transforms a single variable to a new frame.
+     286             :      *
+     287             :      * A convenience override for shared pointers to const.
+     288             :      *
+     289             :      * \param what The object to be transformed.
+     290             :      * \param to_frame The target fram ID.
+     291             :      *
+     292             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     293             :      */
+     294             :     template <class T>
+     295           1 :     [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const boost::shared_ptr<const T>& what, const std::string& to_frame)
+     296             :     {
+     297           2 :       auto ret = transformSingle(*what, to_frame);
+     298           1 :       if (ret == std::nullopt)
+     299           0 :         return std::nullopt;
+     300             :       else
+     301           1 :         return boost::make_shared<T>(std::move(ret.value()));
+     302             :     }
+     303             : 
+     304             :     /**
+     305             :      * \brief Transforms a single variable to a new frame and returns it or \p std::nullopt if transformation fails.
+     306             :      *
+     307             :      * A convenience overload for headerless variables.
+     308             :      *
+     309             :      * \param from_frame the original target frame ID.
+     310             :      * \param what the object to be transformed.
+     311             :      * \param to_frame the target frame ID.
+     312             :      * \param time_stamp the time of the transformation.
+     313             :      *
+     314             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     315             :      */
+     316             :     template <class T>
+     317             :     [[nodiscard]] std::optional<T> transformSingle(const std::string& from_frame, const T& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
+     318             : 
+     319             :     /**
+     320             :      * \brief Transforms a single variable to a new frame and returns it or \p std::nullopt if transformation fails.
+     321             :      *
+     322             :      * A convenience overload for shared pointers to headerless variables.
+     323             :      *
+     324             :      * \param from_frame the original target frame ID.
+     325             :      * \param what the object to be transformed.
+     326             :      * \param to_frame the target frame ID.
+     327             :      * \param time_stamp the time of the transformation.
+     328             :      *
+     329             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     330             :      */
+     331             :     template <class T>
+     332             :     [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const std::string& from_frame, const boost::shared_ptr<T>& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0))
+     333             :     {
+     334             :       return transformSingle(from_frame, boost::shared_ptr<const T>(what), to_frame, time_stamp);
+     335             :     }
+     336             : 
+     337             :     /**
+     338             :      * \brief Transforms a single variable to a new frame and returns it or \p std::nullopt if transformation fails.
+     339             :      *
+     340             :      * A convenience overload for shared pointers to const headerless variables.
+     341             :      *
+     342             :      * \param from_frame the original target frame ID.
+     343             :      * \param what the object to be transformed.
+     344             :      * \param to_frame the target frame ID.
+     345             :      * \param time_stamp the time of the transformation.
+     346             :      *
+     347             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     348             :      */
+     349             :     template <class T>
+     350           1 :     [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const std::string& from_frame, const boost::shared_ptr<const T>& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0))
+     351             :     {
+     352           1 :       auto ret = transformSingle(from_frame, *what, to_frame, time_stamp);
+     353           1 :       if (ret == std::nullopt)
+     354           0 :         return std::nullopt;
+     355             :       else
+     356           1 :         return boost::make_shared<T>(std::move(ret.value()));
+     357             :     }
+     358             : 
+     359             :     //}
+     360             : 
+     361             :     /* transform() //{ */
+     362             : 
+     363             :     /**
+     364             :      * \brief Transform a variable to new frame using a particular transformation.
+     365             :      *
+     366             :      * \param tf The transformation to be used.
+     367             :      * \param what The object to be transformed.
+     368             :      *
+     369             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     370             :      */
+     371             :     template <class T>
+     372             :     [[nodiscard]] std::optional<T> transform(const T& what, const geometry_msgs::TransformStamped& tf);
+     373             : 
+     374             :     /**
+     375             :      * \brief Transform a variable to new frame using a particular transformation.
+     376             :      *
+     377             :      * A convenience override for shared pointers to const.
+     378             :      *
+     379             :      * \param tf The transformation to be used.
+     380             :      * \param what The object to be transformed.
+     381             :      *
+     382             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     383             :      */
+     384             :     template <class T>
+     385           0 :     [[nodiscard]] std::optional<boost::shared_ptr<T>> transform(const boost::shared_ptr<const T>& what, const geometry_msgs::TransformStamped& tf)
+     386             :     {
+     387           0 :       auto ret = transform(*what, tf);
+     388           0 :       if (ret == std::nullopt)
+     389           0 :         return std::nullopt;
+     390             :       else
+     391           0 :         return boost::make_shared<T>(std::move(ret.value()));
+     392             :     }
+     393             : 
+     394             :     /**
+     395             :      * \brief Transform a variable to new frame using a particular transformation.
+     396             :      *
+     397             :      * A convenience override for shared pointers.
+     398             :      *
+     399             :      * \param tf The transformation to be used.
+     400             :      * \param what The object to be transformed.
+     401             :      *
+     402             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     403             :      */
+     404             :     template <class T>
+     405           0 :     [[nodiscard]] std::optional<boost::shared_ptr<T>> transform(const boost::shared_ptr<T>& what, const geometry_msgs::TransformStamped& tf)
+     406             :     {
+     407           0 :       return transform(boost::shared_ptr<const T>(what), tf);
+     408             :     }
+     409             : 
+     410             :     //}
+     411             : 
+     412             :     /* transformAsVector() method //{ */
+     413             :     /**
+     414             :      * \brief Transform an Eigen::Vector3d (interpreting it as a vector).
+     415             :      *
+     416             :      * Only the rotation will be applied to the variable.
+     417             :      *
+     418             :      * \param tf The transformation to be used.
+     419             :      * \param what The vector to be transformed.
+     420             :      *
+     421             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     422             :      */
+     423             :     [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const Eigen::Vector3d& what, const geometry_msgs::TransformStamped& tf);
+     424             : 
+     425             :     /**
+     426             :      * \brief Transform an Eigen::Vector3d (interpreting it as a vector).
+     427             :      *
+     428             :      * Only the rotation will be applied to the variable.
+     429             :      *
+     430             :      * \param from_frame  The current frame of \p what.
+     431             :      * \param what        The vector to be transformed.
+     432             :      * \param to_frame    The desired frame of \p what.
+     433             :      * \param time_stamp  From which time to take the transformation (use \p ros::Time(0) for the latest time).
+     434             :      *
+     435             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     436             :      */
+     437             :     [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
+     438             :     //}
+     439             : 
+     440             :     /* transformAsPoint() method //{ */
+     441             :     /**
+     442             :      * \brief Transform an Eigen::Vector3d (interpreting it as a point).
+     443             :      *
+     444             :      * Both the rotation and translation will be applied to the variable.
+     445             :      *
+     446             :      * \param tf The transformation to be used.
+     447             :      * \param what The object to be transformed.
+     448             :      *
+     449             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     450             :      */
+     451             :     [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const Eigen::Vector3d& what, const geometry_msgs::TransformStamped& tf);
+     452             : 
+     453             :     /**
+     454             :      * \brief Transform an Eigen::Vector3d (interpreting it as a point).
+     455             :      *
+     456             :      * Both the rotation and translation will be applied to the variable.
+     457             :      *
+     458             :      * \param from_frame  The current frame of \p what.
+     459             :      * \param what The object to be transformed.
+     460             :      * \param to_frame    The desired frame of \p what.
+     461             :      * \param time_stamp  From which time to take the transformation (use \p ros::Time(0) for the latest time).
+     462             :      *
+     463             :      * \return \p std::nullopt if failed, optional containing the transformed object otherwise.
+     464             :      */
+     465             :     [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
+     466             :     //}
+     467             : 
+     468             :     /* getTransform() //{ */
+     469             : 
+     470             :     /**
+     471             :      * \brief Obtains a transform between two frames in a given time.
+     472             :      *
+     473             :      * \param from_frame The original frame of the transformation.
+     474             :      * \param to_frame The target frame of the transformation.
+     475             :      * \param time_stamp The time stamp of the transformation. (0 will get the latest)
+     476             :      *
+     477             :      * \return \p std::nullopt if failed, optional containing the requested transformation otherwise.
+     478             :      */
+     479             :     [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransform(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
+     480             : 
+     481             :     /**
+     482             :      * \brief Obtains a transform between two frames in a given time.
+     483             :      *
+     484             :      * This overload enables the user to select a different time of the source frame and the target frame.
+     485             :      *
+     486             :      * \param from_frame The original frame of the transformation.
+     487             :      * \param from_stamp The time at which the original frame should be evaluated. (0 will get the latest)
+     488             :      * \param to_frame The target frame of the transformation.
+     489             :      * \param to_stamp The time to which the data should be transformed. (0 will get the latest) 
+     490             :      * \param fixed_frame The frame that may be assumed constant in time (the "world" frame).
+     491             :      *
+     492             :      * \return \p std::nullopt if failed, optional containing the requested transformation otherwise.
+     493             :      *
+     494             :      * \note An example of when this API may be useful is explained here: http://wiki.ros.org/tf2/Tutorials/Time%20travel%20with%20tf2%20%28C%2B%2B%29
+     495             :      */
+     496             :     [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransform(const std::string& from_frame, const ros::Time& from_stamp, const std::string& to_frame, const ros::Time& to_stamp, const std::string& fixed_frame);
+     497             : 
+     498             :     //}
+     499             : 
+     500             :   private:
+     501             :     /* private members, methods etc //{ */
+     502             : 
+     503             :     std::mutex mutex_;
+     504             : 
+     505             :     // keeps track whether a non-basic constructor was called and the transform listener is initialized
+     506             :     bool initialized_ = false;
+     507             :     std::string node_name_;
+     508             : 
+     509             :     std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+     510             :     std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
+     511             : 
+     512             :     // user-configurable options
+     513             :     std::string default_frame_id_ = "";
+     514             :     std::string prefix_ = ""; // if not empty, includes the forward slash
+     515             :     bool quiet_ = false;
+     516             :     ros::Duration lookup_timeout_ = ros::Duration(0);
+     517             :     bool retry_lookup_newest_ = false;
+     518             : 
+     519             :     bool got_utm_zone_ = false;
+     520             :     std::array<char, 10> utm_zone_ = {};
+     521             : 
+     522             :     // returns the first namespace prefix of the frame (if any) includin the forward slash
+     523             :     std::string getFramePrefix(const std::string& frame_id);
+     524             : 
+     525             :     template <class T>
+     526             :     std::optional<T> transformImpl(const geometry_msgs::TransformStamped& tf, const T& what);
+     527             :     std::optional<mrs_msgs::ReferenceStamped> transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what);
+     528             :     std::optional<Eigen::Vector3d> transformImpl(const geometry_msgs::TransformStamped& tf, const Eigen::Vector3d& what);
+     529             : 
+     530             :     [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransformImpl(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp, const std::string& latlon_frame);
+     531             :     [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransformImpl(const std::string& from_frame, const ros::Time& from_stamp, const std::string& to_frame, const ros::Time& to_stamp, const std::string& fixed_frame, const std::string& latlon_frame);
+     532             : 
+     533             :     std::string resolveFrameImpl(const std::string& frame_id);
+     534             : 
+     535             :     template <class T>
+     536             :     std::optional<T> doTransform(const T& what, const geometry_msgs::TransformStamped& tf);
+     537             : 
+     538             :     //}
+     539             : 
+     540             :     // | ------------------- some helper methods ------------------ |
+     541             :   public:
+     542             :     /* frame_from(), frame_to() and inverse() methods //{ */
+     543             :     
+     544             :     /**
+     545             :      * \brief A convenience function that returns the frame from which the message transforms.
+     546             :      *
+     547             :      * \param msg the message representing the transformation.
+     548             :      * \return    the frame from which the transformation in \msg transforms.
+     549             :      */
+     550     1943523 :     static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
+     551             :     {
+     552     1943523 :       return msg.child_frame_id;
+     553             :     }
+     554             :     
+     555             :     /**
+     556             :      * \brief A convenience function that returns the frame from which the message transforms.
+     557             :      *
+     558             :      * This overload returns a reference to the string in the message representing the frame id so that it can be modified.
+     559             :      *
+     560             :      * \param msg the message representing the transformation.
+     561             :      * \return    a reference to the field in the message containing the string with the frame id from which the transformation in \msg transforms.
+     562             :      */
+     563     1934431 :     static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
+     564             :     {
+     565     1934431 :       return msg.child_frame_id;
+     566             :     }
+     567             :     
+     568             :     /**
+     569             :      * \brief A convenience function that returns the frame to which the message transforms.
+     570             :      *
+     571             :      * \param msg the message representing the transformation.
+     572             :      * \return    the frame to which the transformation in \msg transforms.
+     573             :      */
+     574     1943518 :     static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
+     575             :     {
+     576     1943518 :       return msg.header.frame_id;
+     577             :     }
+     578             : 
+     579             :     /**
+     580             :      * \brief A convenience function that returns the frame to which the message transforms.
+     581             :      *
+     582             :      * This overload returns a reference to the string in the message representing the frame id so that it can be modified.
+     583             :      *
+     584             :      * \param msg the message representing the transformation.
+     585             :      * \return    a reference to the field in the message containing the string with the frame id to which the transformation in \msg transforms.
+     586             :      */
+     587     1937200 :     static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
+     588             :     {
+     589     1937200 :       return msg.header.frame_id;
+     590             :     }
+     591             : 
+     592             :     /**
+     593             :      * \brief A convenience function implements returns the inverse of the transform message as a one-liner.
+     594             :      *
+     595             :      * \param msg the message representing the transformation.
+     596             :      * \return    a new message representing an inverse of the original transformation.
+     597             :      */
+     598           1 :     static geometry_msgs::TransformStamped inverse(const geometry_msgs::TransformStamped& msg)
+     599             :     {
+     600           1 :       tf2::Transform tf2;
+     601           1 :       tf2::fromMsg(msg.transform, tf2);
+     602           1 :       tf2 = tf2.inverse();
+     603           2 :       return create_transform(frame_to(msg), frame_from(msg), msg.header.stamp, tf2::toMsg(tf2));
+     604             :     }
+     605             :     //}
+     606             : 
+     607             :   private:
+     608             :     /* create_transform() method //{ */
+     609             :     static geometry_msgs::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp)
+     610             :     {
+     611             :       geometry_msgs::TransformStamped ret;
+     612             :       frame_from(ret) = from_frame;
+     613             :       frame_to(ret) = to_frame;
+     614             :       ret.header.stamp = time_stamp;
+     615             :       return ret;
+     616             :     }
+     617             : 
+     618     1934488 :     static geometry_msgs::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp, const geometry_msgs::Transform& tf)
+     619             :     {
+     620     1934488 :       geometry_msgs::TransformStamped ret;
+     621     1934383 :       frame_from(ret) = from_frame;
+     622     1934472 :       frame_to(ret) = to_frame;
+     623     1934433 :       ret.header.stamp = time_stamp;
+     624     1934433 :       ret.transform = tf;
+     625     1934433 :       return ret;
+     626             :     }
+     627             :     //}
+     628             : 
+     629             :     /* copyChangeFrame() and related methods //{ */
+     630             : 
+     631             :     // helper type and member for detecting whether a message has a header using SFINAE
+     632             :     template<typename T>
+     633             :     using has_header_member_chk = decltype( std::declval<T&>().header );
+     634             :     template<typename T>
+     635             :     static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
+     636             :     
+     637             :     template <typename msg_t>
+     638             :     std_msgs::Header getHeader(const msg_t& msg);
+     639             :     template <typename pt_t>
+     640             :     std_msgs::Header getHeader(const pcl::PointCloud<pt_t>& cloud);
+     641             :     
+     642             :     template <typename msg_t>
+     643             :     void setHeader(msg_t& msg, const std_msgs::Header& header);
+     644             :     template <typename pt_t>
+     645             :     void setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::Header& header);
+     646             :     
+     647             :     template <typename T>
+     648             :     T copyChangeFrame(const T& what, const std::string& frame_id);
+     649             :     
+     650             :     //}
+     651             : 
+     652             :     /* methods for converting between lattitude/longitude and UTM coordinates //{ */
+     653             :     geometry_msgs::Point LLtoUTM(const geometry_msgs::Point& what, const std::string& prefix);
+     654             :     geometry_msgs::PointStamped LLtoUTM(const geometry_msgs::PointStamped& what, const std::string& prefix);
+     655             :     geometry_msgs::Pose LLtoUTM(const geometry_msgs::Pose& what, const std::string& prefix);
+     656             :     geometry_msgs::PoseStamped LLtoUTM(const geometry_msgs::PoseStamped& what, const std::string& prefix);
+     657             :     
+     658             :     std::optional<geometry_msgs::Point> UTMtoLL(const geometry_msgs::Point& what, const std::string& prefix);
+     659             :     std::optional<geometry_msgs::PointStamped> UTMtoLL(const geometry_msgs::PointStamped& what, const std::string& prefix);
+     660             :     std::optional<geometry_msgs::Pose> UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix);
+     661             :     std::optional<geometry_msgs::PoseStamped> UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix);
+     662             :     
+     663             :     // helper types and member for detecting whether the UTMtoLL and LLtoUTM methods are defined for a certain message
+     664             :     template<class Class, typename Message>
+     665             :     using UTMLL_method_chk = decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(), ""));
+     666             :     template<class Class, typename Message>
+     667             :     using LLUTM_method_chk = decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(), ""));
+     668             :     template<class Class, typename Message>
+     669             :     static constexpr bool UTMLL_exists_v = std::experimental::is_detected<UTMLL_method_chk, Class, Message>::value && std::experimental::is_detected<LLUTM_method_chk, Class, Message>::value;
+     670             :     //}
+     671             : 
+     672             :   };
+     673             : 
+     674             :   //}
+     675             : 
+     676             : }  // namespace mrs_lib
+     677             : 
+     678             : #include <mrs_lib/impl/transformer.hpp>
+     679             : 
+     680             : #endif  // TRANSFORMER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html b/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html new file mode 100644 index 0000000000..5aceca5a90 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html @@ -0,0 +1,190 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/transformer.h.gcov.png b/mrs_lib/include/mrs_lib/transformer.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..27e11b7f6dacb8da8eb743f04a00020774870f30 GIT binary patch literal 2328 zcmV+z3Fr2SP)+1pw=pWW&!q*Gr-mC)Dl3+}z0sGqSFbmd_p%$#a=njTw zXXgmBpt(gF3&M8oQwi=LU{f8AAZ!r1i{~SDt=teC8K*|{sBRyy5$vhoA`bd#YK=br zFT~dLGzVS^9_(IQ_34OK!9LMi({rq&glHBt^wO&OtY{~QF|Q80?R3p*eqd)Z)ii3G z^9vi*-pLIkcoD6v)ex-hx)m%|!)kD+Ph@~#wxX@kMC<}yLkdc0&?23Mej~WPZIc<7 zYU^PD+^w8XQLdE}s%#I4Um21E5y?j$!I>fmEj_7}njG^%V?@d?_XN*ph25k$W<7J{ z9g{(SmK8DQ*E+5sl7|(91nHk-^y$0&MG_S11mU6bjS1CJw9AA7^AiJ(C%50k!eh;6 z88Qcti>F6#ee8&(rO`0VHkxwRgyUhUr$OjLl!iRv92oKfN_^lz5pwKtTZV9muNrnG zwsjy0@%1c&hup>a;NP4F7$65l$f#COCMvOnMomS2_`S3qQnqD11rp;#le4vjaY3 zcUMw9!cJb8eR|x5tlHSeH;NLsTOam&MT)al%w;lC@P0W`=B)OR9)_!&9<=+YGfz%L z$>VbrFbIwoM5nvfVTDJAp1ioyLW)f^^OR{?`?NuPy_HLyt&E{SSas<2>a&G8E zG^Si0L>V%z47@G~;}5M0cTTJ*WM7)ZWmN;jZxotO4A0)sOtB(o2a3uZ30+BCj1iP8 zFv&tGnD-?r`Jop;qE2kYFY%cmpuKEw%4gQk_srpnrq^zKQabrclgsRGm^SlB?!@i81x`^c54?4GD z9Y{hvZo*2RV;+G=A-kkT@BafSpjUqIsD213C`R4i>vk_bdc)prgNNVU{5wrYdpbUb zI?ep$ejlH|V(f4y`jXAXCT`kT(2*&_MY!%oIWtP#ppYbsZo@oRks zyHgyt`?W{)bF4EIPjuO%J-9yNBTabCPV;AxA_cv*{r#-l-{O50{A&n$6dyl6KR)F6 zzTn6D^IpikAT*bVn~`fl3rsRweZ(xPVkv69Y6BRv+&W5D4Pw)wm7OHy| zsm^=Rd_`Q~xe9rI`W@JF?KPUi-O7o4BtdH*H6Lj~Zu0Us_B7cOI(_aVzMieq6qq7~ zqbBtuVh^$Bf!9gd5xvCaTV*J}54dMwL96OU>9bvgY*aMcwaU7pSqRb%#O@1NcGwd< zcMbJR@g<9_{Y!`sH6hOj6_EuVnC+$Znb04v&L7+lj_9bS zymC{oZuOC~N9bMDC$d_IOPjc1^Jy#C z0rn2Y6miXhKGk6Za=P&4sGnMd76t1ZL*JZkCUMR zNotdQcX20i95pj&FXh5Lp;DE5!cP@Sf{wcj==xW6XTf`Qntc8;+kxPtFBv@Bgp%0< zx?zwvjs%yHA`lIrQp&fK&5)vbkx2gbN0EYkZMod92X1|WUl&)u%$u-x*bAEQNBQ-k zdn&#QDO9e&Q@dPhLM^G3MlQN|O{m&!>)11Q0}h@}QwlOsn(|4IAKtK!*cpwm^ZwBL z3u}uGBqg1V?KX7> ztze7=(IIx9t1x_=Ty^vAZQx-AqQS4n&X0o6a+h!90{Q_xSls67Agu|wFyJ0_(GM=& zL)2StIcE?z01iZ|2!#q*H`k~I(&*Om9-4<}#9h@DqD;h8Nz*YH5KRy@naU#_gV~n9 zw$O8f*_jDVvIt7)bg}6ocLkkkNtcV0$TAiv-XhN?&f{jTW9417yEcT>WYvgU?I`aW zb=|gI2(8j*rwc8?jZmOd8r!zj?lobc(5RlQ1jVbU29;g6`J}o6 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - ukf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:040.0 %
Date:2024-04-25 05:42:57Functions:020.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UKF<4, 1, 2>::inverse_exception::what() const0
mrs_lib::UKF<4, 1, 2>::square_root_exception::what() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/ukf.h.func.html b/mrs_lib/include/mrs_lib/ukf.h.func.html new file mode 100644 index 0000000000..06266a52a2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/ukf.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - ukf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:040.0 %
Date:2024-04-25 05:42:57Functions:020.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UKF<4, 1, 2>::inverse_exception::what() const0
mrs_lib::UKF<4, 1, 2>::square_root_exception::what() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/ukf.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/ukf.h.gcov.frameset.html new file mode 100644 index 0000000000..931e85c953 --- /dev/null +++ b/mrs_lib/include/mrs_lib/ukf.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/ukf.h.gcov.html b/mrs_lib/include/mrs_lib/ukf.h.gcov.html new file mode 100644 index 0000000000..a96f879a33 --- /dev/null +++ b/mrs_lib/include/mrs_lib/ukf.h.gcov.html @@ -0,0 +1,285 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - ukf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:040.0 %
Date:2024-04-25 05:42:57Functions:020.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : #ifndef UKF_H
+       3             : #define UKF_H
+       4             : 
+       5             : /**  \file
+       6             :      \brief Defines UKF - a class implementing the Unscented Kalman Filter \cite UKF.
+       7             :      \author Tomáš Báča - bacatoma@fel.cvut.cz (original implementation)
+       8             :      \author Matouš Vrba - vrbamato@fel.cvut.cz (rewrite, documentation)
+       9             :  */
+      10             : 
+      11             : #include <mrs_lib/kalman_filter.h>
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15             : 
+      16             :   /**
+      17             :   * \brief Implementation of the Unscented Kalman filter \cite UKF.
+      18             :   *
+      19             :   * The Unscented Kalman filter (abbreviated UKF, \cite UKF) is a variant of the Kalman filter, which may be used
+      20             :   * for state filtration or estimation of non-linear systems as opposed to the Linear Kalman Filter
+      21             :   * (which is implemented in \ref LKF). The UKF tends to be more accurate than the simpler Extended Kalman Filter,
+      22             :   * espetially for highly non-linear systems. However, it is generally less stable than the LKF because of the extra
+      23             :   * matrix square root in the sigma points calculation, so it is recommended to use LKF for linear systems.
+      24             :   *
+      25             :   * The UKF C++ class itself is templated. This has its advantages and disadvantages. Main disadvantage is that it
+      26             :   * may be harder to use if you're not familiar with C++ templates, which, admittedly, can get somewhat messy,
+      27             :   * espetially during compilation. Another disadvantage is that if used unwisely, the compilation times can get
+      28             :   * much higher when using templates. The main advantage is compile-time checking (if it compiles, then it has
+      29             :   * a lower chance of crashing at runtime) and enabling more effective optimalizations during compilation. Also in case
+      30             :   * of Eigen, the code is arguably more readable when you use aliases to the specific Matrix instances instead of
+      31             :   * having Eigen::MatrixXd and Eigen::VectorXd everywhere.
+      32             :   *
+      33             :   * \tparam n_states         number of states of the system (length of the \f$ \mathbf{x} \f$ vector).
+      34             :   * \tparam n_inputs         number of inputs of the system (length of the \f$ \mathbf{u} \f$ vector).
+      35             :   * \tparam n_measurements   number of measurements of the system (length of the \f$ \mathbf{z} \f$ vector).
+      36             :   *
+      37             :   */
+      38             :   template <int n_states, int n_inputs, int n_measurements>
+      39             :   class UKF : KalmanFilter<n_states, n_inputs, n_measurements>
+      40             :   {
+      41             :   protected:
+      42             :     /* protected UKF definitions (typedefs, constants etc) //{ */
+      43             :     static constexpr int n = n_states;            /*!< \brief Length of the state vector of the system. */
+      44             :     static constexpr int m = n_inputs;            /*!< \brief Length of the input vector of the system. */
+      45             :     static constexpr int p = n_measurements;      /*!< \brief Length of the measurement vector of the system. */
+      46             :     static constexpr int w = 2 * n + 1;           /*!< \brief Number of sigma points/weights. */
+      47             : 
+      48             :     using Base_class = KalmanFilter<n, m, p>; /*!< \brief Base class of this class. */
+      49             : 
+      50             :     using X_t = typename Eigen::Matrix<double, n, w>;    /*!< \brief State sigma points matrix. */
+      51             :     using Z_t = typename Eigen::Matrix<double, p, w>;    /*!< \brief Measurement sigma points matrix. */
+      52             :     using Pzz_t = typename Eigen::Matrix<double, p, p>;  /*!< \brief Pzz helper matrix. */
+      53             :     using K_t = typename Eigen::Matrix<double, n, p>;    /*!< \brief Kalman gain matrix. */
+      54             :     //}
+      55             : 
+      56             :   public:
+      57             :     /* public UKF definitions (typedefs, constants etc) //{ */
+      58             :     //! state vector n*1 typedef
+      59             :     using x_t = typename Base_class::x_t;
+      60             :     //! input vector m*1 typedef
+      61             :     using u_t = typename Base_class::u_t;
+      62             :     //! measurement vector p*1 typedef
+      63             :     using z_t = typename Base_class::z_t;
+      64             :     //! state covariance n*n typedef
+      65             :     using P_t = typename Base_class::P_t;
+      66             :     //! measurement covariance p*p typedef
+      67             :     using R_t = typename Base_class::R_t;
+      68             :     //! process covariance n*n typedef
+      69             :     using Q_t = typename Base_class::Q_t;
+      70             :     //! weights vector (2n+1)*1 typedef
+      71             :     using W_t = typename Eigen::Matrix<double, w, 1>;
+      72             :     //! typedef of a helper struct for state and covariance
+      73             :     using statecov_t = typename Base_class::statecov_t;
+      74             :     //! function of the state transition model typedef
+      75             :     using transition_model_t = typename std::function<x_t(const x_t&, const u_t&, double)>;
+      76             :     //! function of the observation model typedef
+      77             :     using observation_model_t = typename std::function<z_t(const x_t&)>;
+      78             : 
+      79             :     //! is thrown when taking the square root of a matrix fails during sigma generation
+      80             :     struct square_root_exception : public std::exception
+      81             :     {
+      82           0 :       const char* what() const throw()
+      83             :       {
+      84           0 :         return "UKF: taking the square root of covariance update produced NANs!!!";
+      85             :       }
+      86             :     };
+      87             : 
+      88             :     //! is thrown when taking the inverse of a matrix fails during kalman gain calculation
+      89             :     struct inverse_exception : public std::exception
+      90             :     {
+      91           0 :       const char* what() const throw()
+      92             :       {
+      93           0 :         return "UKF: inverting of Pzz in correction update produced NANs!!!";
+      94             :       }
+      95             :     };
+      96             :     //}
+      97             : 
+      98             :   public:
+      99             :     /* UKF constructor //{ */
+     100             :   /*!
+     101             :     * \brief Convenience default constructor.
+     102             :     *
+     103             :     * This constructor should not be used if applicable. If used, the main constructor has to be called afterwards,
+     104             :     * otherwise the UKF object is invalid (not initialized).
+     105             :     */
+     106             :     UKF();
+     107             : 
+     108             :   /*!
+     109             :     * \brief The main constructor.
+     110             :     *
+     111             :     * \param alpha             Scaling parameter of the sigma generation (a small positive value, e.g. 1e-3).
+     112             :     * \param kappa             Secondary scaling parameter of the sigma generation (usually set to 0 or 1).
+     113             :     * \param beta              Incorporates prior knowledge about the distribution (for Gaussian distribution, 2 is optimal).
+     114             :     * \param transition_model  State transition model function.
+     115             :     * \param observation_model Observation model function.
+     116             :     */
+     117             :     UKF(const transition_model_t& transition_model, const observation_model_t& observation_model, const double alpha = 1e-3, const double kappa = 1, const double beta = 2);
+     118             :     //}
+     119             : 
+     120             :     /* correct() method //{ */
+     121             :   /*!
+     122             :     * \brief Implements the state correction step (measurement update).
+     123             :     *
+     124             :     * \param sc     Previous estimate of the state and covariance.
+     125             :     * \param z      Measurement vector.
+     126             :     * \param R      Measurement covariance matrix.
+     127             :     * \returns      The state and covariance after applying the correction step.
+     128             :     */
+     129             :     virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const override;
+     130             :     //}
+     131             : 
+     132             :     /* predict() method //{ */
+     133             :   /*!
+     134             :     * \brief Implements the state prediction step (time update).
+     135             :     *
+     136             :     * \param sc     Previous estimate of the state and covariance.
+     137             :     * \param u      Input vector.
+     138             :     * \param Q      Process noise covariance matrix.
+     139             :     * \param dt     Duration since the previous estimate.
+     140             :     * \returns      The state and covariance after applying the correction step.
+     141             :     */
+     142             :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override;
+     143             :     //}
+     144             : 
+     145             :     /* setConstants() method //{ */
+     146             :   /*!
+     147             :     * \brief Changes the Unscented Transform parameters.
+     148             :     *
+     149             :     * \param alpha  Scaling parameter of the sigma generation (a small positive value - e.g. 1e-3).
+     150             :     * \param kappa  Secondary scaling parameter of the sigma generation (usually set to 0 or 1).
+     151             :     * \param beta   Incorporates prior knowledge about the distribution (for Gaussian distribution, 2 is optimal).
+     152             :     */
+     153             :     void setConstants(const double alpha, const double kappa, const double beta);
+     154             :     //}
+     155             : 
+     156             :     /* setTransitionModel() method //{ */
+     157             :   /*!
+     158             :     * \brief Changes the transition model function.
+     159             :     *
+     160             :     * \param transition_model   the new transition model
+     161             :     */
+     162             :     void setTransitionModel(const transition_model_t& transition_model);
+     163             :     //}
+     164             : 
+     165             :     /* setObservationModel() method //{ */
+     166             :   /*!
+     167             :     * \brief Changes the observation model function.
+     168             :     *
+     169             :     * \param observation_model   the new observation model
+     170             :     */
+     171             :     void setObservationModel(const observation_model_t& observation_model);
+     172             :     //}
+     173             : 
+     174             :   protected:
+     175             :     /* protected methods and member variables //{ */
+     176             : 
+     177             :     void computeWeights();
+     178             : 
+     179             :     X_t computeSigmas(const x_t& x, const P_t& P) const;
+     180             : 
+     181             :     P_t computePaSqrt(const P_t& P) const;
+     182             : 
+     183             :     Pzz_t computeInverse(const Pzz_t& Pzz) const;
+     184             : 
+     185             :     virtual K_t computeKalmanGain([[maybe_unused]] const x_t& x, [[maybe_unused]] const z_t& inn, const K_t& Pxz, const Pzz_t& Pzz) const;
+     186             : 
+     187             :     double m_alpha, m_kappa, m_beta, m_lambda;
+     188             :     W_t m_Wm;
+     189             :     W_t m_Wc;
+     190             : 
+     191             :     transition_model_t m_transition_model;
+     192             :     observation_model_t m_observation_model;
+     193             : 
+     194             :     //}
+     195             :   };
+     196             : 
+     197             : }  // namespace mrs_lib
+     198             : 
+     199             : #include <mrs_lib/impl/ukf.hpp>
+     200             : 
+     201             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/ukf.h.gcov.overview.html b/mrs_lib/include/mrs_lib/ukf.h.gcov.overview.html new file mode 100644 index 0000000000..8e89cb65e2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/ukf.h.gcov.overview.html @@ -0,0 +1,71 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/ukf.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/ukf.h.gcov.png b/mrs_lib/include/mrs_lib/ukf.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5ff0c195cf01462559c2d215ff73531ccba8b968 GIT binary patch literal 1009 zcmVVZnw*eF6UkbNPcKv^=)Z-0Q! zJpOCb3;)gt!h)(=>rYJ;mpx*?e&gkd>qz1kmlx2?w3*=waK{Re25iE{aA|0?LwBLB zD%286Hparp@?@rt*(q~@O)Lu-sI9YR762H%)eEeP>jsec>m_g>I57TPquSJ;^7_)G zl~uzFsa&#*h;7`MbptD>6a$h1=STrU&7%M#>-co5=@WT@1j~La&RkO*Q0pZH+5e`h%4MY>6srN0- z$SR+XQ6J4*Zz%@v&ysnrBU*_=R!G&-Dz#a~HFva!`Vn0Iuh;AId6&=USNVKiv#|r; z((dppW8bhOc$fSvrjGpTb*(`lNUriGOv_55oW(XQVH_PG4w zavd8@E@8?sqP-$Qx2>QKxbfj@wVfG&N8MbqsLd4+EA-zj7wcyGZ3zS=3>1Hv;(>w9#UIiu@RVGaezSpwS0PV2k z(d4><$_;we9s$V^YEQ6@<#4xsGlXYRyd1(=7i3_TvC?$w#2Fc7^oOxY`xK_H3cf6- zmjyrAP;3_k-`~47%$<=@MlX1mbQ0MD{K8ow`7)K2)ZO--xlP4qhR@Y{KBz~5=NRwW z>UW-cfWF13i@wVuJ1gbPCTY1E@dJWVze0>rV8+INwVVJOiEd6EJzn3NW*i7|x_U}X fFW?ibQFVU<_#_im^V@F%00000NkvXXu0mjfeC5`c literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/utils.h.func-sort-c.html b/mrs_lib/include/mrs_lib/utils.h.func-sort-c.html new file mode 100644 index 0000000000..820fbc8ca7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - utils.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1515100.0 %
Date:2024-04-25 05:42:57Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<boost::array<int, 3ul> >(boost::array<int, 3ul> const&, char const*)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > > >(__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, __gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, char const*)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > > >(__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, __gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<int const*>(int const*, int const*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
int mrs_lib::signum<double>(double)44885
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/utils.h.func.html b/mrs_lib/include/mrs_lib/utils.h.func.html new file mode 100644 index 0000000000..e3480692a7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - utils.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1515100.0 %
Date:2024-04-25 05:42:57Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<boost::array<int, 3ul> >(boost::array<int, 3ul> const&, char const*)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > > >(__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, __gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, char const*)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > > >(__gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, __gnu_cxx::__normal_iterator<int*, std::vector<int, std::allocator<int> > >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::containerToString<int const*>(int const*, int const*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
int mrs_lib::signum<double>(double)44885
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/utils.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/utils.h.gcov.frameset.html new file mode 100644 index 0000000000..aa11b08511 --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/utils.h.gcov.html b/mrs_lib/include/mrs_lib/utils.h.gcov.html new file mode 100644 index 0000000000..9b52069844 --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.gcov.html @@ -0,0 +1,229 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - utils.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1515100.0 %
Date:2024-04-25 05:42:57Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines various general utility functions.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :      \author Viktor Walter - walver.viktor@fel.cvut.cz
+       6             :      \author Tomáš Báča - baca.tomas@fel.cvut.cz
+       7             :  */
+       8             : #ifndef UTILS_H
+       9             : #define UTILS_H
+      10             : 
+      11             : #include <iterator>
+      12             : #include <vector>
+      13             : #include <sstream>
+      14             : #include <atomic>
+      15             : 
+      16             : namespace mrs_lib
+      17             : {
+      18             :   /* containerToString() function //{ */
+      19             : 
+      20             :   /**
+      21             :    * \brief Convenience function for converting container ranges to strings (e.g. for printing).
+      22             :    *
+      23             :    * \param begin        first element of the container that will be converted to \p std::string.
+      24             :    * \param end          one-after-the-last element of the container that will be converted to \p std::string.
+      25             :    * \param delimiter    will be used to separate the elements in the output.
+      26             :    * \return             elements of the container from \p begin to \p end (excluding), converted to string and separated by \p delimiter.
+      27             :    *
+      28             :    */
+      29             :   template <typename Iterator>
+      30           2 :   std::string containerToString(const Iterator begin, const Iterator end, const std::string& delimiter = ", ")
+      31             :   {
+      32           2 :     bool first = true;
+      33           4 :     std::ostringstream output;
+      34           7 :     for (Iterator it = begin; it != end; it++)
+      35             :     {
+      36           5 :       if (!first)
+      37           3 :         output << delimiter;
+      38           5 :       output << *it;
+      39           5 :       first = false;
+      40             :     }
+      41           4 :     return output.str();
+      42             :   }
+      43             : 
+      44             :   /**
+      45             :    * \brief Convenience function for converting container ranges to strings (e.g. for printing).
+      46             :    *
+      47             :    * \param begin        first element of the container that will be converted to \p std::string.
+      48             :    * \param end          one-after-the-last element of the container that will be converted to \p std::string.
+      49             :    * \param delimiter    will be used to separate the elements in the output.
+      50             :    * \return             elements of the container from \p begin to \p end (excluding), converted to string and separated by \p delimiter.
+      51             :    *
+      52             :    */
+      53             :   template <typename Iterator>
+      54           1 :   std::string containerToString(const Iterator begin, const Iterator end, const char* delimiter)
+      55             :   {
+      56           1 :     return containerToString(begin, end, std::string(delimiter));
+      57             :   }
+      58             : 
+      59             :   /**
+      60             :    * \brief Convenience function for converting containers to strings (e.g. for printing).
+      61             :    *
+      62             :    * \param cont         the container that will be converted to \p std::string.
+      63             :    * \param delimiter    will be used to separate the elements in the output.
+      64             :    * \return             elements of the container from \p begin to \p end (excluding), converted to string and separated by \p delimiter.
+      65             :    *
+      66             :    */
+      67             :   template <typename Container>
+      68             :   std::string containerToString(const Container& cont, const std::string& delimiter = ", ")
+      69             :   {
+      70             :     return containerToString(std::begin(cont), std::end(cont), delimiter);
+      71             :   }
+      72             : 
+      73             :   /**
+      74             :    * \brief Convenience function for converting containers to strings (e.g. for printing).
+      75             :    *
+      76             :    * \param cont         the container that will be converted to \p std::string.
+      77             :    * \param delimiter    will be used to separate the elements in the output.
+      78             :    * \return             elements of the container from \p begin to \p end (excluding), converted to string and separated by \p delimiter.
+      79             :    *
+      80             :    */
+      81             :   template <typename Container>
+      82           1 :   std::string containerToString(const Container& cont, const char* delimiter = ", ")
+      83             :   {
+      84           1 :     return containerToString(std::begin(cont), std::end(cont), std::string(delimiter));
+      85             :   }
+      86             : 
+      87             :   //}
+      88             : 
+      89             :   /* remove_const() function //{ */
+      90             : 
+      91             :   /**
+      92             :    * \brief Convenience class for removing const-ness from a container iterator.
+      93             :    *
+      94             :    * \param it    the iterator from which const-ness should be removed.
+      95             :    * \param cont  the corresponding container of the iterator.
+      96             :    * \return      a non-const iterator, pointing to the same element as \p it.
+      97             :    *
+      98             :    */
+      99             :   template <typename T>
+     100             :   typename T::iterator remove_const(const typename T::const_iterator& it, T& cont)
+     101             :   {
+     102             :     typename T::iterator ret = cont.begin();
+     103             :     std::advance(ret, std::distance((typename T::const_iterator)ret, it));
+     104             :     return ret;
+     105             :   }
+     106             : 
+     107             :   //}
+     108             : 
+     109             :   /**
+     110             :    * \brief Convenience class for automatically setting and unsetting an atomic boolean based on the object's scope.
+     111             :    * Useful e.g. for indicating whether a thread is running or not.
+     112             :    *
+     113             :    */
+     114             :   class AtomicScopeFlag
+     115             :   {
+     116             : 
+     117             :   public:
+     118             :   /**
+     119             :    * \brief The constructor. Sets the flag \p in to \p true.
+     120             :    *
+     121             :    * \param in  The flag to be set on construction of this object and reset (set to \p false) on its destruction.
+     122             :    *
+     123             :    */
+     124             :     AtomicScopeFlag(std::atomic<bool>& in);
+     125             :   /**
+     126             :    * \brief The destructor. Resets the variable given in the constructor to \p false.
+     127             :    *
+     128             :    */
+     129             :     ~AtomicScopeFlag();
+     130             : 
+     131             :   private:
+     132             :     std::atomic<bool>& variable;
+     133             :   };
+     134             : 
+     135             :   // branchless, templated, more efficient version of the signum function
+     136             :   // taken from https://stackoverflow.com/questions/1903954/is-there-a-standard-sign-function-signum-sgn-in-c-c
+     137             :   template <typename T>
+     138       44885 :   int signum(T val)
+     139             :   {
+     140       44885 :     return (T(0) < val) - (val < T(0));
+     141             :   }
+     142             : 
+     143             : }  // namespace mrs_lib
+     144             : 
+     145             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/utils.h.gcov.overview.html b/mrs_lib/include/mrs_lib/utils.h.gcov.overview.html new file mode 100644 index 0000000000..d983ecb00f --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/utils.h.gcov.png b/mrs_lib/include/mrs_lib/utils.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3c5a1ba70c3b1da9d74597a8f6a3c0e7615a62c4 GIT binary patch literal 677 zcmV;W0$TlvP)l=4-G#Um_Yy`w6Rs=M&%v{=o`r-?%xM=OW>h&bqcU+TQz!t_l+ia=f`g{ zR3ebC>F<(@nc_|$XwYCW585V*GUF4_YnnA?2Y?xjsxXB|u>b-jM|J$g9D4!O!}y3( z>5n>1RXeh2o;lbn0z%RcBZC=;F0pmW%D84()+*GV>CMgDn&J{&GQ~*U=k)Y}ED75N zXW&vio5`7>J*xQp>7;&<(HwNLONQ3&6{vH)_G$2 z%brso?`BUU_fSR|oC)j;ukm1<{Ww+)E zv_~BY1xYIs0xvt6FdPj^V}SR0$bcsq-sw?u)~2MeU&YCx{HQABB;Ewpy!6`E%Y?EX zeaj!rxc6&3Vz|_MN;%5mz?GsVc{G)%nYbrtChOJd@xaP>uO0>`9EE$x`x^!6hQOmK zMHJ~S3CLLmhtCS`i41#bM9^E)gF*O)CiT%EpB>CyJZk2ow&~6} + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/vector_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - vector_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:3636100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, pcl::PointXYZ>(pcl::PointXYZ const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, pcl::PointXYZ>(pcl::PointXYZ const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, pcl::PointXYZ>(pcl::PointXYZ const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, pcl::PointXYZ>(pcl::PointXYZ const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, pcl::PointXYZ>(pcl::PointXYZ const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, pcl::PointXYZ>(pcl::PointXYZ const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/vector_converter.h.func.html b/mrs_lib/include/mrs_lib/vector_converter.h.func.html new file mode 100644 index 0000000000..9ee1a41ca7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/vector_converter.h.func.html @@ -0,0 +1,224 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/vector_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - vector_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:3636100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, pcl::PointXYZ>(pcl::PointXYZ const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::convert<geometry_msgs::Vector3_<std::allocator<void> >, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, pcl::PointXYZ>(pcl::PointXYZ const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
cv::Vec<double, 3> mrs_lib::convert<cv::Vec<double, 3>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, pcl::PointXYZ>(pcl::PointXYZ const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
cv::Vec<float, 3> mrs_lib::convert<cv::Vec<float, 3>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
pcl::PointXYZ mrs_lib::convert<pcl::PointXYZ, pcl::PointXYZ>(pcl::PointXYZ const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, pcl::PointXYZ>(pcl::PointXYZ const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
Eigen::Matrix<double, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, cv::Vec<double, 3> >(cv::Vec<double, 3> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, cv::Vec<float, 3> >(cv::Vec<float, 3> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, pcl::PointXYZ>(pcl::PointXYZ const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)1
Eigen::Matrix<float, 3, 1, 0, 3, 1> mrs_lib::convert<Eigen::Matrix<float, 3, 1, 0, 3, 1>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&)1
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/vector_converter.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/vector_converter.h.gcov.frameset.html new file mode 100644 index 0000000000..e28918f7a4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/vector_converter.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/vector_converter.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/vector_converter.h.gcov.html b/mrs_lib/include/mrs_lib/vector_converter.h.gcov.html new file mode 100644 index 0000000000..cbe7951607 --- /dev/null +++ b/mrs_lib/include/mrs_lib/vector_converter.h.gcov.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/vector_converter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - vector_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:3636100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file vector_converter.h
+       3             :      \brief Defines the convert() function for conversion between different vector representations (Eigen, OpenCV, tf2 etc.).
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : #ifndef VECTOR_CONVERTER_H
+       7             : #define VECTOR_CONVERTER_H
+       8             : 
+       9             : #include <tuple>
+      10             : 
+      11             : #include <mrs_lib/impl/vector_converter.hpp>
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15             : 
+      16             :   /*!
+      17             :    * \brief Converts between different vector representations.
+      18             :    *
+      19             :    * Usage (for full example, see the file vector_converter/example.cpp):
+      20             :    *
+      21             :    * `auto out = mrs_lib::convert<to_type>(in);`
+      22             :    *
+      23             :    * Supported types by default are: `Eigen::Vector3d`, `cv::Vec3d`, `geometry_msgs::Vector3`.
+      24             :    * If you want to use this with a new type, it should work automagically if it provides a reasonable API.
+      25             :    * If it doesn't work by default, you just need to implement the respective convertTo() and convertFrom() functions for that type (see the file impl/vector_converter.hpp and the example file vector_converter/example.cpp).
+      26             :    *
+      27             :    * \param in  The input vector to be converted to the \p ret_t type.
+      28             :    *
+      29             :    * \tparam ret_t Type of the return vector. You need to specify this when calling the function.
+      30             :    * \tparam in_t  Type of the input vector. This parameter is deduced by the compiler and doesn't need to be specified in usual cases.
+      31             :    *
+      32             :    */
+      33             :   template <typename ret_t, typename in_t>
+      34          36 :   ret_t convert(const in_t& in)
+      35             :   {
+      36          36 :     const auto [x, y, z] = impl::convertFrom(in);
+      37          36 :     ret_t ret;
+      38          36 :     impl::convertTo(ret, x, y, z);
+      39          66 :     return ret;
+      40             :   }
+      41             : 
+      42             : }
+      43             : 
+      44             : #endif // VECTOR_CONVERTER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/vector_converter.h.gcov.overview.html b/mrs_lib/include/mrs_lib/vector_converter.h.gcov.overview.html new file mode 100644 index 0000000000..943e1c27f6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/vector_converter.h.gcov.overview.html @@ -0,0 +1,31 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/vector_converter.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/vector_converter.h.gcov.png b/mrs_lib/include/mrs_lib/vector_converter.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..0213761517fcd503a7734a61558dfce4281808fe GIT binary patch literal 366 zcmeAS@N?(olHy`uVBq!ia0vp^0YI$7!VDxC?klMPNr?cT5ZC|z|E~gqG_SB2M%>2yfa=luuJVf}zG{gTHW~)`*f}N$NhxBgI-QBu0 zUgYBYbF77KfvcK&J?5I!9x!5v+P_nwa?;v~tZ(h7W^8_X?C;`pk5hW98`P%gY{>b< z7T;ibT_bhtakp5upzE)WbRLzQUAZ<%?dy@5MS0FK{10b+p2m8+F!wdXX{$e1g+q;= zZh1Oo)8c;pBQuf?Dq3*`CP~d+ab(3zgM0SXH*URr$?%sUM$Ps3p-Hp7fZ@R4>FVdQ I&MBb@03ZyU+yDRo literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/visual_object.h.func-sort-c.html b/mrs_lib/include/mrs_lib/visual_object.h.func-sort-c.html new file mode 100644 index 0000000000..634a9f9cb6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/visual_object.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/visual_object.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - visual_object.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::VisualObject::operator<(mrs_lib::VisualObject const&) const242
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.func.html b/mrs_lib/include/mrs_lib/visual_object.h.func.html new file mode 100644 index 0000000000..e4038cd6c3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/visual_object.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/visual_object.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - visual_object.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::VisualObject::operator<(mrs_lib::VisualObject const&) const242
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/visual_object.h.gcov.frameset.html new file mode 100644 index 0000000000..c9a9cb38a2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/visual_object.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/visual_object.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.gcov.html b/mrs_lib/include/mrs_lib/visual_object.h.gcov.html new file mode 100644 index 0000000000..5c25085278 --- /dev/null +++ b/mrs_lib/include/mrs_lib/visual_object.h.gcov.html @@ -0,0 +1,186 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/visual_object.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - visual_object.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Object abstraction for the Batch Visualizer
+       3             :      \author Petr Štibinger - stibipet@fel.cvut.cz
+       4             :  */
+       5             : 
+       6             : #ifndef BATCH_VISUALIZER__VISUAL_OBJECT_H
+       7             : #define BATCH_VISUALIZER__VISUAL_OBJECT_H
+       8             : 
+       9             : #include <Eigen/Core>
+      10             : #include <ros/time.h>
+      11             : #include <std_msgs/ColorRGBA.h>
+      12             : #include <geometry_msgs/Point.h>
+      13             : #include <mrs_lib/geometry/shapes.h>
+      14             : #include <mrs_msgs/Path.h>
+      15             : #include <mrs_msgs/TrajectoryReference.h>
+      16             : 
+      17             : #define DEFAULT_ELLIPSE_POINTS 64
+      18             : 
+      19             : namespace mrs_lib
+      20             : {
+      21             : 
+      22             : enum MarkerType
+      23             : {
+      24             :   POINT    = 0,
+      25             :   LINE     = 1,
+      26             :   TRIANGLE = 2
+      27             : };
+      28             : 
+      29             : class VisualObject {
+      30             : 
+      31             : 
+      32             : public:
+      33             :   VisualObject(const Eigen::Vector3d& point, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      34             :                const unsigned long& id);
+      35             : 
+      36             :   VisualObject(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      37             :                const unsigned long& id);
+      38             : 
+      39             :   VisualObject(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      40             :                const bool filled, const unsigned long& id);
+      41             : 
+      42             :   VisualObject(const mrs_lib::geometry::Rectangle& rectangle, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      43             :                const bool filled, const unsigned long& id);
+      44             : 
+      45             :   VisualObject(const mrs_lib::geometry::Cuboid& cuboid, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      46             :                const bool filled, const unsigned long& id);
+      47             : 
+      48             :   VisualObject(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      49             :                const bool filled, const unsigned long& id, const int num_points = DEFAULT_ELLIPSE_POINTS);
+      50             : 
+      51             :   VisualObject(const mrs_lib::geometry::Cylinder& cylinder, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      52             :                const bool filled, const bool capped, const unsigned long& id, const int num_sides = DEFAULT_ELLIPSE_POINTS);
+      53             : 
+      54             :   VisualObject(const mrs_lib::geometry::Cone& cone, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      55             :                const bool filled, const bool capped, const unsigned long& id, const int num_sides = DEFAULT_ELLIPSE_POINTS);
+      56             : 
+      57             :   VisualObject(const mrs_msgs::Path& p, const double r, const double g, const double b, const double a, const ros::Duration& timeout, const bool filled,
+      58             :                const unsigned long& id);
+      59             : 
+      60             :   VisualObject(const mrs_msgs::TrajectoryReference& traj, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      61             :                const bool filled, const unsigned long& id);
+      62             : 
+      63             : 
+      64             : public:
+      65             :   unsigned long getID() const;
+      66             :   int           getType() const;
+      67             :   bool          isTimedOut() const;
+      68             : 
+      69             :   const std::vector<geometry_msgs::Point> getPoints() const;
+      70             :   const std::vector<std_msgs::ColorRGBA>  getColors() const;
+      71             : 
+      72         242 :   bool operator<(const VisualObject& other) const {
+      73         242 :     return id_ < other.id_;
+      74             :   }
+      75             : 
+      76             :   bool operator>(const VisualObject& other) const {
+      77             :     return id_ > other.id_;
+      78             :   }
+      79             : 
+      80             :   bool operator==(const VisualObject& other) const {
+      81             :     return id_ == other.id_;
+      82             :   }
+      83             : 
+      84             : private:
+      85             :   const unsigned long               id_;
+      86             :   MarkerType                        type_;
+      87             :   std::vector<geometry_msgs::Point> points_;
+      88             :   std::vector<std_msgs::ColorRGBA>  colors_;
+      89             :   ros::Time                         timeout_time_;
+      90             : 
+      91             :   void addRay(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a);
+      92             : 
+      93             :   void addTriangle(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a, const bool filled);
+      94             : 
+      95             :   void addEllipse(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a, const bool filled,
+      96             :                   const int num_points);
+      97             : 
+      98             : };  // namespace batch_visualizer
+      99             : 
+     100             : }  // namespace mrs_lib
+     101             : 
+     102             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html b/mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html new file mode 100644 index 0000000000..d12f1bdebc --- /dev/null +++ b/mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html @@ -0,0 +1,46 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/visual_object.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.gcov.png b/mrs_lib/include/mrs_lib/visual_object.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2e10038ce016599658d08255c1ff1a58c48536b4 GIT binary patch literal 493 zcmV+saxyxDMeS`bZ>XVXLfMj6TyyoCqrtw2lm0u z+o(BT9)Ca$kB$N})=&j%ZYAN~M-2fl?xgk-O5E?%?$Wmj&*fS>a()0krI)3KDBPQ< zbwrxU^|#FxFTAkosWR(uv15M}_ueAI2C<4{orGw`5gG}@y*-=U|C$HgxkA&LlV~x;9avy^{ zD4UE5G>^=dcMGuSb&_NEgJF^XJ)xiRS!%!{##?Fl?i#WPsloKLN2!F_$XG*)sD*#d joDtA2469xit&98vAi`i14~K$;00000NkvXXu0mjfj#t*F literal 0 HcmV?d00001 diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html new file mode 100644 index 0000000000..256162e5c6 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::AttitudeConverter::setYaw(double const&)0
mrs_lib::Vector3Converter::operator tf2::Vector3() const0
mrs_lib::EulerAttitude::EulerAttitude(double const&, double const&, double const&)1
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getRoll()1
mrs_lib::AttitudeConverter::getPitch()1
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(mrs_lib::EulerAttitude const&)1
mrs_lib::EulerAttitude::yaw() const1
mrs_lib::EulerAttitude::roll() const1
mrs_lib::EulerAttitude::pitch() const1
mrs_lib::Vector3Converter::operator geometry_msgs::Vector3_<std::allocator<void> >() const1
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator mrs_lib::EulerAttitude() const1
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)23127
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)104963
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()138591
mrs_lib::AttitudeConverter::getYaw()182463
mrs_lib::AttitudeConverter::calculateRPY()182468
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)276294
mrs_lib::AttitudeConverter::setHeading(double const&)285313
mrs_lib::AttitudeConverter::getVectorZ()287306
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)299425
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const373631
mrs_lib::AttitudeConverter::operator tf2::Transform() const390258
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)448909
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const586740
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const867521
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)928041
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1204015
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1493641
mrs_lib::AttitudeConverter::getHeading()1582879
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)3508628
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)7146905
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const8332165
mrs_lib::AttitudeConverter::validateOrientation()13234907
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html new file mode 100644 index 0000000000..96518457d0 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::EulerAttitude::EulerAttitude(double const&, double const&, double const&)1
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)276294
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)23127
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getHeading()1582879
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::getVectorZ()287306
mrs_lib::AttitudeConverter::setHeading(double const&)285313
mrs_lib::AttitudeConverter::calculateRPY()182468
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)299425
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)104963
mrs_lib::AttitudeConverter::validateOrientation()13234907
mrs_lib::AttitudeConverter::getYaw()182463
mrs_lib::AttitudeConverter::setYaw(double const&)0
mrs_lib::AttitudeConverter::getRoll()1
mrs_lib::AttitudeConverter::getPitch()1
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)3508628
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)928041
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)448909
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1204015
mrs_lib::AttitudeConverter::AttitudeConverter(mrs_lib::EulerAttitude const&)1
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)7146905
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()138591
mrs_lib::EulerAttitude::yaw() const1
mrs_lib::EulerAttitude::roll() const1
mrs_lib::EulerAttitude::pitch() const1
mrs_lib::Vector3Converter::operator geometry_msgs::Vector3_<std::allocator<void> >() const1
mrs_lib::Vector3Converter::operator tf2::Vector3() const0
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const586740
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const8332165
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const867521
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const373631
mrs_lib::AttitudeConverter::operator tf2::Transform() const390258
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1493641
mrs_lib::AttitudeConverter::operator mrs_lib::EulerAttitude() const1
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.frameset.html b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.frameset.html new file mode 100644 index 0000000000..4736f83ffd --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html new file mode 100644 index 0000000000..3e1f22a2a2 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html @@ -0,0 +1,561 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: TomasFormat
+       2             : 
+       3             : #include <mrs_lib/attitude_converter.h>
+       4             : #include <mrs_lib/utils.h>
+       5             : 
+       6             : using quat_t = Eigen::Quaterniond;
+       7             : 
+       8             : namespace mrs_lib
+       9             : {
+      10             : 
+      11             : /* class EulerAttitude //{ */
+      12             : 
+      13           1 : EulerAttitude::EulerAttitude(const double& roll, const double& pitch, const double& yaw) : roll_(roll), pitch_(pitch), yaw_(yaw) {
+      14           1 : }
+      15             : 
+      16           1 : double EulerAttitude::roll(void) const {
+      17           1 :   return roll_;
+      18             : }
+      19             : 
+      20           1 : double EulerAttitude::pitch(void) const {
+      21           1 :   return pitch_;
+      22             : }
+      23             : 
+      24           1 : double EulerAttitude::yaw(void) const {
+      25           1 :   return yaw_;
+      26             : }
+      27             : 
+      28             : //}
+      29             : 
+      30             : /* class Vector3Converter //{ */
+      31             : 
+      32       23127 : Vector3Converter::Vector3Converter(const Eigen::Vector3d& vector3) {
+      33             : 
+      34       23127 :   vector3_[0] = vector3[0];
+      35       23127 :   vector3_[1] = vector3[1];
+      36       23127 :   vector3_[2] = vector3[2];
+      37       23127 : }
+      38             : 
+      39      276294 : Vector3Converter::Vector3Converter(const geometry_msgs::Vector3& vector3) {
+      40             : 
+      41      276265 :   vector3_[0] = vector3.x;
+      42      276262 :   vector3_[1] = vector3.y;
+      43      276231 :   vector3_[2] = vector3.z;
+      44      276247 : }
+      45             : 
+      46           1 : Vector3Converter::Vector3Converter(const double& x, const double& y, const double& z) {
+      47             : 
+      48           1 :   vector3_[0] = x;
+      49           1 :   vector3_[1] = y;
+      50           1 :   vector3_[2] = z;
+      51           1 : }
+      52             : 
+      53           0 : Vector3Converter::operator tf2::Vector3() const {
+      54             : 
+      55           0 :   return vector3_;
+      56             : }
+      57             : 
+      58      586740 : Vector3Converter::operator Eigen::Vector3d() const {
+      59             : 
+      60      586740 :   return Eigen::Vector3d(vector3_[0], vector3_[1], vector3_[2]);
+      61             : }
+      62             : 
+      63           1 : Vector3Converter::operator geometry_msgs::Vector3() const {
+      64             : 
+      65           1 :   geometry_msgs::Vector3 vector_3;
+      66             : 
+      67           1 :   vector_3.x = vector3_[0];
+      68           1 :   vector_3.y = vector3_[1];
+      69           1 :   vector_3.z = vector3_[2];
+      70             : 
+      71           1 :   return vector_3;
+      72             : }
+      73             : 
+      74             : //}
+      75             : 
+      76             : /* constructors //{ */
+      77             : 
+      78     7146905 : AttitudeConverter::AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format) {
+      79             : 
+      80     7144769 :   switch (format) {
+      81             : 
+      82     7145182 :     case RPY_EXTRINSIC: {
+      83     7145182 :       tf2_quaternion_.setRPY(roll, pitch, yaw);
+      84     7145162 :       break;
+      85             :     }
+      86             : 
+      87           5 :     case RPY_INTRINSIC: {
+      88             : 
+      89           5 :       Eigen::Matrix3d Y, P, R;
+      90             : 
+      91           5 :       Y << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1;
+      92             : 
+      93           5 :       P << cos(pitch), 0, sin(pitch), 0, 1, 0, -sin(pitch), 0, cos(pitch);
+      94             : 
+      95           5 :       R << 1, 0, 0, 0, cos(roll), -sin(roll), 0, sin(roll), cos(roll);
+      96             : 
+      97           5 :       tf2_quaternion_ = AttitudeConverter(R * P * Y);
+      98             : 
+      99           5 :       break;
+     100             :     }
+     101             :   }
+     102             : 
+     103     7144759 :   validateOrientation();
+     104     7145874 : }
+     105             : 
+     106           1 : AttitudeConverter::AttitudeConverter(const tf::Quaternion quaternion) {
+     107             : 
+     108           1 :   tf2_quaternion_.setX(quaternion.x());
+     109           1 :   tf2_quaternion_.setY(quaternion.y());
+     110           1 :   tf2_quaternion_.setZ(quaternion.z());
+     111           1 :   tf2_quaternion_.setW(quaternion.w());
+     112             : 
+     113           1 :   validateOrientation();
+     114           1 : }
+     115             : 
+     116     3508628 : AttitudeConverter::AttitudeConverter(const geometry_msgs::Quaternion quaternion) {
+     117     3508412 :   tf2_quaternion_.setX(quaternion.x);
+     118     3507913 :   tf2_quaternion_.setY(quaternion.y);
+     119     3508488 :   tf2_quaternion_.setZ(quaternion.z);
+     120     3508601 :   tf2_quaternion_.setW(quaternion.w);
+     121             : 
+     122     3508687 :   validateOrientation();
+     123     3507751 : }
+     124             : 
+     125           1 : AttitudeConverter::AttitudeConverter(const mrs_lib::EulerAttitude& euler_attitude) {
+     126           1 :   tf2_quaternion_.setRPY(euler_attitude.roll(), euler_attitude.pitch(), euler_attitude.yaw());
+     127             : 
+     128           1 :   validateOrientation();
+     129           1 : }
+     130             : 
+     131      448909 : AttitudeConverter::AttitudeConverter(const Eigen::Quaterniond quaternion) {
+     132      448909 :   tf2_quaternion_.setX(quaternion.x());
+     133      448909 :   tf2_quaternion_.setY(quaternion.y());
+     134      448909 :   tf2_quaternion_.setZ(quaternion.z());
+     135      448909 :   tf2_quaternion_.setW(quaternion.w());
+     136             : 
+     137      448909 :   validateOrientation();
+     138      448909 : }
+     139             : 
+     140     1204015 : AttitudeConverter::AttitudeConverter(const Eigen::Matrix3d matrix) {
+     141     1202036 :   Eigen::Quaterniond quaternion = Eigen::Quaterniond(matrix);
+     142             : 
+     143     1202363 :   tf2_quaternion_.setX(quaternion.x());
+     144     1201108 :   tf2_quaternion_.setY(quaternion.y());
+     145     1202313 :   tf2_quaternion_.setZ(quaternion.z());
+     146     1202249 :   tf2_quaternion_.setW(quaternion.w());
+     147             : 
+     148     1202265 :   validateOrientation();
+     149     1199069 : }
+     150             : 
+     151      928041 : AttitudeConverter::AttitudeConverter(const tf2::Quaternion quaternion) {
+     152             : 
+     153      926367 :   tf2_quaternion_ = quaternion;
+     154             : 
+     155      926367 :   validateOrientation();
+     156      926130 : }
+     157             : 
+     158           1 : AttitudeConverter::AttitudeConverter(const tf2::Matrix3x3 matrix) {
+     159             : 
+     160           1 :   matrix.getRotation(tf2_quaternion_);
+     161             : 
+     162           1 :   validateOrientation();
+     163           1 : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* operators //{ */
+     168             : 
+     169      867521 : AttitudeConverter::operator tf2::Quaternion() const {
+     170      867521 :   return tf2_quaternion_;
+     171             : }
+     172             : 
+     173           1 : AttitudeConverter::operator tf::Quaternion() const {
+     174             : 
+     175           1 :   tf::Quaternion tf_quaternion;
+     176             : 
+     177           1 :   tf_quaternion.setX(tf2_quaternion_.x());
+     178           1 :   tf_quaternion.setY(tf2_quaternion_.y());
+     179           1 :   tf_quaternion.setZ(tf2_quaternion_.z());
+     180           1 :   tf_quaternion.setW(tf2_quaternion_.w());
+     181             : 
+     182           1 :   return tf_quaternion;
+     183             : }
+     184             : 
+     185     8332165 : AttitudeConverter::operator geometry_msgs::Quaternion() const {
+     186             : 
+     187     8332165 :   geometry_msgs::Quaternion geom_quaternion;
+     188             : 
+     189     8329462 :   geom_quaternion.x = tf2_quaternion_.x();
+     190     8323465 :   geom_quaternion.y = tf2_quaternion_.y();
+     191     8323975 :   geom_quaternion.z = tf2_quaternion_.z();
+     192     8325786 :   geom_quaternion.w = tf2_quaternion_.w();
+     193             : 
+     194     8324781 :   return geom_quaternion;
+     195             : }
+     196             : 
+     197           1 : AttitudeConverter::operator EulerAttitude() const {
+     198             : 
+     199             :   double roll, pitch, yaw;
+     200           1 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll, pitch, yaw);
+     201             : 
+     202           2 :   return EulerAttitude(roll, pitch, yaw);
+     203             : }
+     204             : 
+     205     1493641 : AttitudeConverter::operator Eigen::Matrix3d() const {
+     206             : 
+     207     1493641 :   Eigen::Quaterniond quaternion(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     208             : 
+     209     2986873 :   return quaternion.toRotationMatrix();
+     210             : }
+     211             : 
+     212      138591 : AttitudeConverter::operator std::tuple<double&, double&, double&>() {
+     213             : 
+     214      138591 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     215             : 
+     216      138591 :   return std::tuple<double&, double&, double&>{roll_, pitch_, yaw_};
+     217             : }
+     218             : 
+     219      373631 : AttitudeConverter::operator tf2::Matrix3x3() const {
+     220             : 
+     221      373631 :   return tf2::Matrix3x3(tf2_quaternion_);
+     222             : }
+     223             : 
+     224      390258 : AttitudeConverter::operator tf2::Transform() const {
+     225             : 
+     226      390258 :   return tf2::Transform(tf2_quaternion_);
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* getters //{ */
+     232             : 
+     233      182463 : double AttitudeConverter::getYaw(void) {
+     234             : 
+     235      182463 :   calculateRPY();
+     236             : 
+     237      182463 :   return yaw_;
+     238             : }
+     239             : 
+     240           1 : double AttitudeConverter::getRoll(void) {
+     241             : 
+     242           1 :   calculateRPY();
+     243             : 
+     244           1 :   return roll_;
+     245             : }
+     246             : 
+     247           1 : double AttitudeConverter::getPitch(void) {
+     248             : 
+     249           1 :   calculateRPY();
+     250             : 
+     251           1 :   return pitch_;
+     252             : }
+     253             : 
+     254     1582879 : double AttitudeConverter::getHeading(void) {
+     255             : 
+     256     1582879 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     257             : 
+     258     1582503 :   tf2::Vector3 x_new = tf2::Transform(tf2_quaternion_) * b1;
+     259             : 
+     260     1581646 :   if (fabs(x_new[0]) <= 1e-3 && fabs(x_new[1]) <= 1e-3) {
+     261           5 :     throw GetHeadingException();
+     262             :   }
+     263             : 
+     264     3160502 :   return atan2(x_new[1], x_new[0]);
+     265             : }
+     266             : 
+     267      104963 : double AttitudeConverter::getYawRateIntrinsic(const double& heading_rate) {
+     268             : 
+     269             :   // when the heading rate is very small, it does not make sense to compute the
+     270             :   // yaw rate (the math would break), return 0
+     271      104963 :   if (fabs(heading_rate) < 1e-3) {
+     272       76140 :     return 0;
+     273             :   }
+     274             : 
+     275             :   // prep
+     276       28823 :   Eigen::Matrix3d R = *this;
+     277             : 
+     278             :   // construct the heading orbital velocity vector
+     279       28823 :   Eigen::Vector3d heading_vector   = Eigen::Vector3d(R(0, 0), R(1, 0), 0);
+     280       28823 :   Eigen::Vector3d orbital_velocity = Eigen::Vector3d(0, 0, heading_rate).cross(heading_vector);
+     281             : 
+     282             :   // projector to the heading orbital velocity vector subspace
+     283       28823 :   Eigen::Vector3d b_orb = Eigen::Vector3d(0, 0, 1).cross(heading_vector);
+     284       28823 :   b_orb.normalize();
+     285       28823 :   Eigen::Matrix3d P = b_orb * b_orb.transpose();
+     286             : 
+     287             :   // project the body yaw orbital velocity vector base onto the heading orbital velocity vector subspace
+     288       28823 :   Eigen::Vector3d projected = P * R.col(1);
+     289             : 
+     290             : 
+     291       28823 :   double orbital_velocity_norm = orbital_velocity.norm();
+     292       28823 :   double projected_norm        = projected.norm();
+     293             : 
+     294       28823 :   if (fabs(projected_norm) < 1e-5) {
+     295           0 :     ROS_ERROR("[AttitudeConverter]: getYawRateIntrinsic(): \"projected_norm\" in denominator is almost zero!!!");
+     296           0 :     throw MathErrorException();
+     297             :   }
+     298             : 
+     299       28823 :   double direction = mrs_lib::signum(orbital_velocity.dot(projected));
+     300             : 
+     301       28823 :   double output_yaw_rate = direction * (orbital_velocity_norm / projected_norm);
+     302             : 
+     303       28823 :   if (!std::isfinite(output_yaw_rate)) {
+     304           0 :     ROS_ERROR("[AttitudeConverter]: getYawRateIntrinsic(): NaN detected in variable \"output_yaw_rate\"!!!");
+     305           0 :     throw MathErrorException();
+     306             :   }
+     307             : 
+     308             :   // extract the yaw rate
+     309       28823 :   return output_yaw_rate;
+     310             : }
+     311             : 
+     312      299425 : double AttitudeConverter::getHeadingRate(const Vector3Converter& attitude_rate) {
+     313             : 
+     314             :   // prep
+     315      299425 :   Eigen::Matrix3d R = *this;
+     316      299406 :   Eigen::Vector3d w = attitude_rate;
+     317             : 
+     318             :   // create the angular velocity tensor
+     319      299332 :   Eigen::Matrix3d W;
+     320      299340 :   W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
+     321             : 
+     322             :   // R derivative
+     323      299337 :   Eigen::Matrix3d R_d = R * W;
+     324             : 
+     325             :   // atan2 derivative
+     326      299405 :   double rx = R(0, 0);  // x-component of body X
+     327      299244 :   double ry = R(1, 0);  // y-component of body Y
+     328             : 
+     329      299310 :   double denom = rx * rx + ry * ry;
+     330             : 
+     331      299310 :   if (fabs(denom) <= 1e-5) {
+     332           0 :     ROS_ERROR("[AttitudeConverter]: getHeadingRate(): denominator near zero!!!");
+     333           0 :     throw MathErrorException();
+     334             :   }
+     335             : 
+     336      299310 :   double atan2_d_x = -ry / denom;
+     337      299310 :   double atan2_d_y = rx / denom;
+     338             : 
+     339             :   // atan2 total differential
+     340      299310 :   double heading_rate = atan2_d_x * R_d(0, 0) + atan2_d_y * R_d(1, 0);
+     341             : 
+     342      299360 :   return heading_rate;
+     343             : }
+     344             : 
+     345           3 : Vector3Converter AttitudeConverter::getVectorX(void) {
+     346             : 
+     347           3 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     348             : 
+     349           3 :   tf2::Vector3 new_b1 = tf2::Transform(tf2_quaternion_) * b1;
+     350             : 
+     351           6 :   return Vector3Converter(new_b1);
+     352             : }
+     353             : 
+     354           3 : Vector3Converter AttitudeConverter::getVectorY(void) {
+     355             : 
+     356           3 :   tf2::Vector3 b2 = tf2::Vector3(0, 1, 0);
+     357             : 
+     358           3 :   tf2::Vector3 new_b2 = tf2::Transform(tf2_quaternion_) * b2;
+     359             : 
+     360           6 :   return Vector3Converter(new_b2);
+     361             : }
+     362             : 
+     363      287306 : Vector3Converter AttitudeConverter::getVectorZ(void) {
+     364             : 
+     365      287306 :   tf2::Vector3 b3 = tf2::Vector3(0, 0, 1);
+     366             : 
+     367      287306 :   tf2::Vector3 new_b3 = tf2::Transform(tf2_quaternion_) * b3;
+     368             : 
+     369      574608 :   return Vector3Converter(new_b3);
+     370             : }
+     371             : 
+     372           1 : std::tuple<double, double, double> AttitudeConverter::getExtrinsicRPY(void) {
+     373             : 
+     374           1 :   Eigen::Matrix3d rot = AttitudeConverter(*this);
+     375             : 
+     376           1 :   Eigen::Vector3d eulers = rot.eulerAngles(2, 1, 0);
+     377             : 
+     378           2 :   return std::tuple(eulers[2], eulers[1], eulers[0]);
+     379             : }
+     380             : 
+     381           1 : std::tuple<double, double, double> AttitudeConverter::getIntrinsicRPY(void) {
+     382             : 
+     383           1 :   Eigen::Matrix3d rot = AttitudeConverter(*this);
+     384             : 
+     385           1 :   Eigen::Vector3d eulers = rot.eulerAngles(0, 1, 2);
+     386             : 
+     387           2 :   return std::tuple(eulers[0], eulers[1], eulers[2]);
+     388             : }
+     389             : 
+     390             : //}
+     391             : 
+     392             : /* setters //{ */
+     393             : 
+     394           0 : AttitudeConverter AttitudeConverter::setYaw(const double& new_yaw) {
+     395             : 
+     396           0 :   auto [roll, pitch, yaw] = *this;
+     397             : 
+     398           0 :   std::ignore = yaw;
+     399             : 
+     400           0 :   return AttitudeConverter(roll, pitch, new_yaw);
+     401             : }
+     402             : 
+     403      285313 : AttitudeConverter AttitudeConverter::setHeading(const double& heading) {
+     404             : 
+     405             :   // get the Z unit vector after the original rotation
+     406      285313 :   Eigen::Vector3d b3 = getVectorZ();
+     407             : 
+     408             :   // check for singularity: z component of the thrust vector is 0
+     409      285303 :   if (fabs(b3[2]) < 1e-3) {
+     410           3 :     throw SetHeadingException();
+     411             :   }
+     412             : 
+     413             :   // get the desired heading as a vector in 3D
+     414      285305 :   Eigen::Vector3d h(cos(heading), sin(heading), 0);
+     415             : 
+     416      285307 :   Eigen::Matrix3d new_R;
+     417             : 
+     418      285302 :   new_R.col(2) = b3;
+     419             : 
+     420             :   // construct the oblique projection
+     421      285306 :   Eigen::Matrix3d projector_body_z_compl = (Eigen::Matrix3d::Identity(3, 3) - b3 * b3.transpose());
+     422             : 
+     423             :   // create a basis of the body-z complement subspace
+     424      569273 :   Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+     425      285048 :   A.col(0)          = projector_body_z_compl.col(0);
+     426      285009 :   A.col(1)          = projector_body_z_compl.col(1);
+     427             : 
+     428             :   // create the basis of the projection null-space complement
+     429      568909 :   Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+     430      285076 :   B.col(0)          = Eigen::Vector3d(1, 0, 0);
+     431      284992 :   B.col(1)          = Eigen::Vector3d(0, 1, 0);
+     432             : 
+     433             :   // oblique projector to <range_basis>
+     434      568323 :   Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     435      568646 :   Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     436      284256 :   Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     437             : 
+     438      284124 :   new_R.col(0) = oblique_projector * h;
+     439      283142 :   new_R.col(0).normalize();
+     440             : 
+     441             :   // | ------------------------- body y ------------------------- |
+     442             : 
+     443      284035 :   new_R.col(1) = new_R.col(2).cross(new_R.col(0));
+     444      282003 :   new_R.col(1).normalize();
+     445             : 
+     446      567588 :   return AttitudeConverter(new_R);
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : // | ------------------------ internal ------------------------ |
+     452             : 
+     453             : /* calculateRPY() //{ */
+     454             : 
+     455      182468 : void AttitudeConverter::calculateRPY(void) {
+     456             : 
+     457      182468 :   if (!got_rpy_) {
+     458             : 
+     459      182468 :     tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     460             :   }
+     461      182468 : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* validateOrientation() //{ */
+     466             : 
+     467    13234907 : void AttitudeConverter::validateOrientation(void) {
+     468             : 
+     469    26460387 :   if (!std::isfinite(tf2_quaternion_.x()) || !std::isfinite(tf2_quaternion_.y()) || !std::isfinite(tf2_quaternion_.z()) ||
+     470    13221159 :       !std::isfinite(tf2_quaternion_.w())) {
+     471           2 :     throw InvalidAttitudeException();
+     472             :   }
+     473    13223849 : }
+     474             : 
+     475             : //}
+     476             : 
+     477             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.overview.html b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.overview.html new file mode 100644 index 0000000000..09e6f1e033 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.overview.html @@ -0,0 +1,140 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.png b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2614538441222d07b06c41c37d10540a6b353539 GIT binary patch literal 1533 zcmV*mt7a)o-F8MFKD~?ZALlQz3x2$Yk`KktA<_v|4 ztgc!^+Y@>WnfeskB{qlEV1Z$Y?GePEHYOBik13<(_^oC zHuJ1BzfJ`_#9fQ(ao5W52Gw&e;Cbp1CVqatO-LP3&-sA!)$<3(%lvwOzTVGxEYHrd zJRPDIU`nD0I2I`cxJcG7+FVBqjfpt^rUyJ^`M@1ISxBgc3lJ6oX&8m!t{oqz0#bOa zN$UZ>Wp$mluuAYeDf&3ix%IkO)4}kwMfH7c^5EaNYhV&>ywEc|4fT$R|0DH}BdPspD zQzd;)ZsvUhc*xii=O*xWfFr_G0neQo_yLg`XXaACUa9Y_aC?oJnHD|0{tu)MA>ilN z4UD~jRfX$ZFXPdyr!smXF!l@0x0^yciF9FF<*Tn(kFNJfDn`V5cJg?kdXCzXPI~R3 zN9?HJLZ*2*u>9&)>fq{(=W%bT3EFBIkczQox|ae{glVZp1D-Y4?o%+z!I|+GF+eg# zxKiH@kRr^T{2F<6*DuUsUY(nF_chXm%swk&jaIJPTq!-7kX=ku^ueg9e?yw%{4*==ctgfluT7N<6Du{}`on;heD!n>rJxFR<;HI7PvpR5_KihlE+)^JjATu}l z>K<4brXG;YlzJLBJB6Q_ZSy#P3boWdaG_)uC3l!Gy*&Clj&1d2R+UB4&cX%Ezm<0w z_ssx-GfHSKbRNB7n09E_7LD=sJl3N$GmiN- zfpqUk63r-@#$hi2s`~+On8z(8T=3WB`TUE=JMU;yH#H#mTe1CYd3JWD4d^jjk77d_ z+X9FTbuiFDgQ>>pEr=c+_#Z!XtsS?VoQDH%W?KZk^}q)HEmE4cN=0^%K~;Tuf2Jp1~4zsrZzXvf|DG_DpP1>*}yYZ=aR z*aHvHr2!lia*4f#raa4-smi+gVp6VC0~}$grjg`i*yh6AoSR~U70qG}6tFEs4p2oeguvZ5pViah`F@!OzUYEjKR+Nrnx&16D$K6apgGa_? ztLFvMuq2H(eRa zm>74!c;X3TG?m}kFd#}uMlp7F&-E|t-_yD8e0oOSUTg!{k$mJBC2}u6`dv1&tcklo j`hS{h&bSK^Rowporw3}RyWQBl00000NkvXXu0mjfO#jd7 literal 0 HcmV?d00001 diff --git a/mrs_lib/src/attitude_converter/index-detail-sort-f.html b/mrs_lib/src/attitude_converter/index-detail-sort-f.html new file mode 100644 index 0000000000..8957887b96 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
<unnamed>94.6 %212 / 22495.1 %39 / 41
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/index-detail-sort-l.html b/mrs_lib/src/attitude_converter/index-detail-sort-l.html new file mode 100644 index 0000000000..932fcce3dd --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
<unnamed>94.6 %212 / 22495.1 %39 / 41
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/index-detail.html b/mrs_lib/src/attitude_converter/index-detail.html new file mode 100644 index 0000000000..1f6917bf9e --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
<unnamed>94.6 %212 / 22495.1 %39 / 41
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/index-sort-f.html b/mrs_lib/src/attitude_converter/index-sort-f.html new file mode 100644 index 0000000000..9c2a4c3a87 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/index-sort-l.html b/mrs_lib/src/attitude_converter/index-sort-l.html new file mode 100644 index 0000000000..563a317d70 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/attitude_converter/index.html b/mrs_lib/src/attitude_converter/index.html new file mode 100644 index 0000000000..a014aeb238 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-25 05:42:57Functions:394195.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.cpp +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html new file mode 100644 index 0000000000..e849b51224 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221774.7 %
Date:2024-04-25 05:42:57Functions:132356.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::BatchVisualizer::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addTrajectory(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setLinesScale(double)0
mrs_lib::BatchVisualizer::addRay(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCone(mrs_lib::geometry::Cone const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addPath(mrs_msgs::Path_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCuboid(mrs_lib::geometry::Cuboid const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setParentFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)22
mrs_lib::BatchVisualizer::setPointsScale(double)22
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)79
mrs_lib::BatchVisualizer::initialize()188
mrs_lib::BatchVisualizer::BatchVisualizer(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)188
mrs_lib::BatchVisualizer::BatchVisualizer()188
mrs_lib::BatchVisualizer::clearBuffers()210
mrs_lib::BatchVisualizer::clearVisuals()210
mrs_lib::BatchVisualizer::~BatchVisualizer()376
mrs_lib::BatchVisualizer::addNullPoint()398
mrs_lib::BatchVisualizer::addNullLine()410
mrs_lib::BatchVisualizer::addNullTriangle()410
mrs_lib::BatchVisualizer::publish()410
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html new file mode 100644 index 0000000000..2a009acfea --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221774.7 %
Date:2024-04-25 05:42:57Functions:132356.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::BatchVisualizer::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::initialize()188
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullLine()410
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullPoint()398
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::clearBuffers()210
mrs_lib::BatchVisualizer::clearVisuals()210
mrs_lib::BatchVisualizer::addTrajectory(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setLinesScale(double)0
mrs_lib::BatchVisualizer::setParentFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)22
mrs_lib::BatchVisualizer::setPointsScale(double)22
mrs_lib::BatchVisualizer::addNullTriangle()410
mrs_lib::BatchVisualizer::addRay(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCone(mrs_lib::geometry::Cone const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addPath(mrs_msgs::Path_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::publish()410
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)79
mrs_lib::BatchVisualizer::addCuboid(mrs_lib::geometry::Cuboid const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::BatchVisualizer(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)188
mrs_lib::BatchVisualizer::BatchVisualizer()188
mrs_lib::BatchVisualizer::~BatchVisualizer()376
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.frameset.html b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.frameset.html new file mode 100644 index 0000000000..5a116ddf18 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html new file mode 100644 index 0000000000..d1a92cf7a2 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html @@ -0,0 +1,440 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221774.7 %
Date:2024-04-25 05:42:57Functions:132356.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <string>
+       2             : #include <mrs_lib/batch_visualizer.h>
+       3             : #include <mrs_lib/geometry/shapes.h>
+       4             : 
+       5             : namespace mrs_lib
+       6             : {
+       7             : 
+       8             : /* constructors */  //{
+       9         188 : BatchVisualizer::BatchVisualizer() {
+      10         188 : }
+      11             : 
+      12         376 : BatchVisualizer::~BatchVisualizer() {
+      13         376 : }
+      14             : 
+      15         188 : BatchVisualizer::BatchVisualizer(ros::NodeHandle &nh, const std::string marker_topic_name, const std::string parent_frame) {
+      16         188 :   this->parent_frame      = parent_frame;
+      17         188 :   this->marker_topic_name = marker_topic_name;
+      18         188 :   initialize();
+      19             : 
+      20         188 :   this->visual_pub = nh.advertise<visualization_msgs::MarkerArray>(marker_topic_name.c_str(), 1);
+      21         188 :   publish();
+      22         188 : }
+      23             : //}
+      24             : 
+      25             : /* setParentFrame //{ */
+      26             : 
+      27          22 : void BatchVisualizer::setParentFrame(const std::string parent_frame) {
+      28          22 :   this->parent_frame               = parent_frame;
+      29          22 :   points_marker.header.frame_id    = parent_frame;
+      30          22 :   triangles_marker.header.frame_id = parent_frame;
+      31          22 :   lines_marker.header.frame_id     = parent_frame;
+      32          22 : }
+      33             : 
+      34             : //}
+      35             : 
+      36             : /* initialize //{ */
+      37         188 : void BatchVisualizer::initialize() {
+      38         188 :   if (initialized) {
+      39           0 :     return;
+      40             :   }
+      41             : 
+      42             :   // setup points marker
+      43         188 :   std::stringstream ss;
+      44         188 :   ss << marker_topic_name << "_points";
+      45         188 :   points_marker.header.frame_id    = parent_frame;
+      46         188 :   points_marker.header.stamp       = ros::Time::now();
+      47         188 :   points_marker.ns                 = ss.str().c_str();
+      48         188 :   points_marker.action             = visualization_msgs::Marker::ADD;
+      49         188 :   points_marker.pose.orientation.w = 1.0;
+      50         188 :   points_marker.id                 = 8;
+      51         188 :   points_marker.type               = visualization_msgs::Marker::POINTS;
+      52             : 
+      53         188 :   points_marker.scale.x = points_scale;
+      54         188 :   points_marker.scale.y = points_scale;
+      55         188 :   points_marker.color.a = 1.0;
+      56             : 
+      57             :   // setup lines marker
+      58         188 :   ss.str(std::string());
+      59         188 :   ss << marker_topic_name << "_lines";
+      60         188 :   lines_marker.header.frame_id    = parent_frame;
+      61         188 :   lines_marker.header.stamp       = ros::Time::now();
+      62         188 :   lines_marker.ns                 = ss.str().c_str();
+      63         188 :   lines_marker.action             = visualization_msgs::Marker::ADD;
+      64         188 :   lines_marker.pose.orientation.w = 1.0;
+      65         188 :   lines_marker.id                 = 5;
+      66         188 :   lines_marker.type               = visualization_msgs::Marker::LINE_LIST;
+      67             : 
+      68         188 :   lines_marker.scale.x = lines_scale;
+      69         188 :   lines_marker.color.a = 1.0;
+      70             : 
+      71             :   // setup triangles marker
+      72         188 :   ss.str(std::string());
+      73         188 :   ss << marker_topic_name << "_triangles";
+      74         188 :   triangles_marker.header.frame_id    = parent_frame;
+      75         188 :   triangles_marker.header.stamp       = ros::Time::now();
+      76         188 :   triangles_marker.ns                 = ss.str().c_str();
+      77         188 :   triangles_marker.action             = visualization_msgs::Marker::ADD;
+      78         188 :   triangles_marker.pose.orientation.w = 1.0;
+      79         188 :   triangles_marker.id                 = 11;
+      80         188 :   triangles_marker.type               = visualization_msgs::Marker::TRIANGLE_LIST;
+      81             : 
+      82         188 :   triangles_marker.scale.x = 1;
+      83         188 :   triangles_marker.scale.y = 1;
+      84         188 :   triangles_marker.scale.z = 1;
+      85         188 :   triangles_marker.color.a = 1.0;
+      86             : 
+      87         188 :   ROS_INFO("[%s]: Batch visualizer loaded with default values", ros::this_node::getName().c_str());
+      88         188 :   initialized = true;
+      89             : }
+      90             : //}
+      91             : 
+      92             : /* addPoint //{ */
+      93          79 : void BatchVisualizer::addPoint(const Eigen::Vector3d &p, const double r, const double g, const double b, const double a, const ros::Duration &timeout) {
+      94             : 
+      95         158 :   VisualObject obj = VisualObject(p, r, g, b, a, timeout, uuid++);
+      96          79 :   visual_objects.insert(obj);
+      97          79 : }
+      98             : //}
+      99             : 
+     100             : /* addRay */  //{
+     101           0 : void BatchVisualizer::addRay(const mrs_lib::geometry::Ray &ray, const double r, const double g, const double b, const double a, const ros::Duration &timeout) {
+     102             : 
+     103           0 :   VisualObject obj = VisualObject(ray, r, g, b, a, timeout, uuid++);
+     104           0 :   visual_objects.insert(obj);
+     105           0 : }
+     106             : //}
+     107             : 
+     108             : /* addTriangle //{ */
+     109           0 : void BatchVisualizer::addTriangle(const mrs_lib::geometry::Triangle &tri, const double r, const double g, const double b, const double a, const bool filled,
+     110             :                                   const ros::Duration &timeout) {
+     111             : 
+     112           0 :   VisualObject obj = VisualObject(tri, r, g, b, a, timeout, filled, uuid++);
+     113           0 :   visual_objects.insert(obj);
+     114           0 : }
+     115             : //}
+     116             : 
+     117             : /* addRectangle //{ */
+     118           0 : void BatchVisualizer::addRectangle(const mrs_lib::geometry::Rectangle &rect, const double r, const double g, const double b, const double a, const bool filled,
+     119             :                                    const ros::Duration &timeout) {
+     120             : 
+     121           0 :   VisualObject obj = VisualObject(rect, r, g, b, a, timeout, filled, uuid++);
+     122           0 :   visual_objects.insert(obj);
+     123           0 : }
+     124             : //}
+     125             : 
+     126             : /* addCuboid //{ */
+     127           0 : void BatchVisualizer::addCuboid(const mrs_lib::geometry::Cuboid &cuboid, const double r, const double g, const double b, const double a, const bool filled,
+     128             :                                 const ros::Duration &timeout) {
+     129             : 
+     130           0 :   VisualObject obj = VisualObject(cuboid, r, g, b, a, timeout, filled, uuid++);
+     131           0 :   visual_objects.insert(obj);
+     132           0 : }
+     133             : //}
+     134             : 
+     135             : /* addEllipse //{ */
+     136           0 : void BatchVisualizer::addEllipse(const mrs_lib::geometry::Ellipse &ellipse, const double r, const double g, const double b, const double a, const bool filled,
+     137             :                                  const int num_points, const ros::Duration &timeout) {
+     138             : 
+     139           0 :   VisualObject obj = VisualObject(ellipse, r, g, b, a, timeout, filled, uuid++, num_points);
+     140           0 :   visual_objects.insert(obj);
+     141           0 : }
+     142             : //}
+     143             : 
+     144             : /* addCylinder //{ */
+     145           0 : void BatchVisualizer::addCylinder(const mrs_lib::geometry::Cylinder &cylinder, const double r, const double g, const double b, const double a,
+     146             :                                   const bool filled, const bool capped, const int sides, const ros::Duration &timeout) {
+     147           0 :   VisualObject obj = VisualObject(cylinder, r, g, b, a, timeout, filled, capped, uuid++, sides);
+     148           0 :   visual_objects.insert(obj);
+     149           0 : }
+     150             : //}
+     151             : 
+     152             : /* addCone //{ */
+     153           0 : void BatchVisualizer::addCone(const mrs_lib::geometry::Cone &cone, const double r, const double g, const double b, const double a, const bool filled,
+     154             :                               const bool capped, const int sides, const ros::Duration &timeout) {
+     155           0 :   VisualObject obj = VisualObject(cone, r, g, b, a, timeout, filled, capped, uuid++, sides);
+     156           0 :   visual_objects.insert(obj);
+     157           0 : }
+     158             : //}
+     159             : 
+     160             : /* addPath //{ */
+     161           0 : void BatchVisualizer::addPath(const mrs_msgs::Path &p, const double r, const double g, const double b, const double a, const bool filled,
+     162             :                               const ros::Duration &timeout) {
+     163           0 :   VisualObject obj = VisualObject(p, r, g, b, a, timeout, filled, uuid++);
+     164           0 :   visual_objects.insert(obj);
+     165           0 : }
+     166             : //}
+     167             : 
+     168             : /* addTrajectory //{ */
+     169           0 : void BatchVisualizer::addTrajectory(const mrs_msgs::TrajectoryReference &traj, const double r, const double g, const double b, const double a,
+     170             :                                     const bool filled, const ros::Duration &timeout) {
+     171           0 :   VisualObject obj = VisualObject(traj, r, g, b, a, timeout, filled, uuid++);
+     172           0 :   visual_objects.insert(obj);
+     173           0 : }
+     174             : //}
+     175             : 
+     176             : /* addNullPoint //{ */
+     177         398 : void BatchVisualizer::addNullPoint() {
+     178         398 :   geometry_msgs::Point p;
+     179         398 :   p.x = 10000.0;
+     180         398 :   p.y = 0.0;
+     181         398 :   p.z = 0.0;
+     182             : 
+     183         398 :   std_msgs::ColorRGBA c;
+     184         398 :   c.r = 1.0;
+     185         398 :   c.g = 1.0;
+     186         398 :   c.b = 1.0;
+     187         398 :   c.a = 1.0;
+     188             : 
+     189         398 :   points_marker.points.push_back(p);
+     190         398 :   points_marker.colors.push_back(c);
+     191         398 : }
+     192             : //}
+     193             : 
+     194             : /* addNullLine //{ */
+     195         410 : void BatchVisualizer::addNullLine() {
+     196         410 :   geometry_msgs::Point p1, p2;
+     197         410 :   p1.x = 10000.0;
+     198         410 :   p1.y = 0.0;
+     199         410 :   p1.z = 0.0;
+     200             : 
+     201         410 :   p2.x = 10001.0;
+     202         410 :   p2.y = 0.0;
+     203         410 :   p2.z = 0.0;
+     204             : 
+     205         410 :   std_msgs::ColorRGBA c;
+     206         410 :   c.r = 1.0;
+     207         410 :   c.g = 1.0;
+     208         410 :   c.b = 1.0;
+     209             : 
+     210         410 :   lines_marker.colors.push_back(c);
+     211         410 :   lines_marker.colors.push_back(c);
+     212             : 
+     213         410 :   lines_marker.points.push_back(p1);
+     214         410 :   lines_marker.points.push_back(p2);
+     215         410 : }
+     216             : //}
+     217             : 
+     218             : /* addNullTriangle //{ */
+     219         410 : void BatchVisualizer::addNullTriangle() {
+     220         410 :   geometry_msgs::Point p1, p2, p3;
+     221         410 :   p1.x = 10000.0;
+     222         410 :   p1.y = 0.0;
+     223         410 :   p1.z = 0.0;
+     224             : 
+     225         410 :   p2.x = 10001.0;
+     226         410 :   p2.y = 0.0;
+     227         410 :   p2.z = 0.0;
+     228             : 
+     229         410 :   std_msgs::ColorRGBA c;
+     230         410 :   c.r = 1.0;
+     231         410 :   c.g = 1.0;
+     232         410 :   c.b = 1.0;
+     233             : 
+     234         410 :   p3.x = 10001.0;
+     235         410 :   p3.y = 0.01;
+     236         410 :   p3.z = 0.0;
+     237         410 :   triangles_marker.colors.push_back(c);
+     238         410 :   triangles_marker.colors.push_back(c);
+     239         410 :   triangles_marker.colors.push_back(c);
+     240             : 
+     241         410 :   triangles_marker.points.push_back(p1);
+     242         410 :   triangles_marker.points.push_back(p2);
+     243         410 :   triangles_marker.points.push_back(p3);
+     244         410 : }
+     245             : //}
+     246             : 
+     247             : /* setPointsScale //{ */
+     248          22 : void BatchVisualizer::setPointsScale(const double scale) {
+     249          22 :   points_scale = scale;
+     250          22 : }
+     251             : //}
+     252             : 
+     253             : /* setLinesScale //{ */
+     254           0 : void BatchVisualizer::setLinesScale(const double scale) {
+     255           0 :   lines_scale = scale;
+     256           0 : }
+     257             : //}
+     258             : 
+     259             : /* clearBuffers //{ */
+     260         210 : void BatchVisualizer::clearBuffers() {
+     261         210 :   visual_objects.clear();
+     262         210 : }
+     263             : //}
+     264             : 
+     265             : /* clearVisuals //{ */
+     266         210 : void BatchVisualizer::clearVisuals() {
+     267         420 :   std::set<VisualObject> visual_objects_tmp;
+     268         210 :   visual_objects_tmp.insert(visual_objects.begin(), visual_objects.end());
+     269             : 
+     270         210 :   visual_objects.clear();
+     271         210 :   publish();
+     272             : 
+     273         210 :   visual_objects.insert(visual_objects_tmp.begin(), visual_objects_tmp.end());
+     274         210 : }
+     275             : //}
+     276             : 
+     277             : /* publish //{ */
+     278         410 : void BatchVisualizer::publish() {
+     279             : 
+     280         410 :   msg.markers.clear();
+     281         410 :   points_marker.points.clear();
+     282         410 :   points_marker.colors.clear();
+     283             : 
+     284         410 :   lines_marker.points.clear();
+     285         410 :   lines_marker.colors.clear();
+     286             : 
+     287         410 :   triangles_marker.points.clear();
+     288         410 :   triangles_marker.colors.clear();
+     289             : 
+     290             :   // fill marker messages and remove objects that have timed out
+     291         466 :   for (auto it = visual_objects.begin(); it != visual_objects.end();) {
+     292          56 :     if (it->isTimedOut()) {
+     293           0 :       it = visual_objects.erase(it);
+     294             :     } else {
+     295         112 :       auto points = it->getPoints();
+     296         112 :       auto colors = it->getColors();
+     297          56 :       switch (it->getType()) {
+     298          56 :         case MarkerType::POINT: {
+     299          56 :           points_marker.points.insert(points_marker.points.end(), points.begin(), points.end());
+     300          56 :           points_marker.colors.insert(points_marker.colors.end(), colors.begin(), colors.end());
+     301          56 :           break;
+     302             :         }
+     303           0 :         case MarkerType::LINE: {
+     304           0 :           lines_marker.points.insert(lines_marker.points.end(), points.begin(), points.end());
+     305           0 :           lines_marker.colors.insert(lines_marker.colors.end(), colors.begin(), colors.end());
+     306           0 :           break;
+     307             :         }
+     308           0 :         case MarkerType::TRIANGLE: {
+     309           0 :           triangles_marker.points.insert(triangles_marker.points.end(), points.begin(), points.end());
+     310           0 :           triangles_marker.colors.insert(triangles_marker.colors.end(), colors.begin(), colors.end());
+     311           0 :           break;
+     312             :         }
+     313             :       }
+     314          56 :       it++;
+     315             :     }
+     316             :   }
+     317             : 
+     318         410 :   auto now = ros::Time::now();
+     319             : 
+     320         410 :   if (!points_marker.points.empty()) {
+     321          12 :     points_marker.scale.x = points_scale;
+     322          12 :     points_marker.scale.y = points_scale;
+     323             :   } else {
+     324         398 :     addNullPoint();
+     325             :   }
+     326         410 :   points_marker.header.stamp = now;
+     327         410 :   msg.markers.push_back(points_marker);
+     328             : 
+     329         410 :   if (!lines_marker.points.empty()) {
+     330           0 :     lines_marker.scale.x = lines_scale;
+     331             :   } else {
+     332         410 :     addNullLine();
+     333             :   }
+     334         410 :   lines_marker.header.stamp = now;
+     335         410 :   msg.markers.push_back(lines_marker);
+     336             : 
+     337         410 :   if (!triangles_marker.points.empty()) {
+     338           0 :     triangles_marker.header.stamp = now;
+     339             :   } else {
+     340         410 :     addNullTriangle();
+     341             :   }
+     342         410 :   triangles_marker.header.stamp = now;
+     343         410 :   msg.markers.push_back(triangles_marker);
+     344             : 
+     345         410 :   if (msg.markers.empty()) {
+     346           0 :     addNullPoint();
+     347           0 :     points_marker.scale.x = 0.1;
+     348           0 :     points_marker.scale.y = 0.1;
+     349           0 :     msg.markers.push_back(points_marker);
+     350             :   }
+     351             : 
+     352         410 :   visual_pub.publish(msg);
+     353         410 : }
+     354             : //}
+     355             : 
+     356             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html new file mode 100644 index 0000000000..4222da3c96 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html @@ -0,0 +1,109 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..589d1dd69d27c156fbd83eb79cd9d9dd81c37ada GIT binary patch literal 1265 zcmV+#se&lDtK+-3^Gtsn^m|xG#8Agq4!f7`=a8$U5a@V)VUr;zei_G4=J>X%pO!)krPgM-N+0BR@@@4mq2rL%fA4?xSImT1L+D|xY)=HV;wS0c)dkn zTg0TyykwEQrv&1`Vq86>e%?_Jct#rl5dE~#43R}E3L4ngPuoUBKd@<_gNgu9w=pkM z>M^oFRdGEKRphucG3%E0gLI#sktz=~z0uEr_zBWQc@HOVZ8K6Cx6*T9w!=KiGzBn` zuZ5GgTHp|mHqt)`CL12TuQi!1j}d4$N)5M>B;1pJC&K9v01#g7vBK3Y;d=zy&3*>q z%2Vzuwanj%a9ZF1;i~y!sBH*G1pXf3AmcY8oNo)bEw{YM)x;Zj zrx3uR*=6u2{6GfitF2XAYAe?`zKDRn^zXL~4>AHn7`^1Hg$kcH21soHUKr zIRNCEHmX>|!{qUk-DjVbiJ}nhDTU%GGr8qTaXL9nr=6&D4Dblg#o{`4aX5e~-RtWG z`=eHO+1>j53Omm`&T};g`?kM$c)wRh1nw45L z4kY{9ym69%+8NXxs*SOopAJ74S*tCG$e&s%t)Ae9#94zc6F{EAdFSaf-*z0t3V&0%R6IxX3D3 z9eB*7>Leb?+-y#dIFoSJ3<8;@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-25 05:42:57Functions:204544.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
13.8%13.8%
+ 		13.8 %30 / 21831.8 %7 / 22
<unnamed>13.8 %30 / 21831.8 %7 / 22
batch_visualizer.cpp +
74.7%74.7%
+ 		74.7 %162 / 21756.5 %13 / 23
<unnamed>74.7 %162 / 21756.5 %13 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-detail-sort-l.html b/mrs_lib/src/batch_visualizer/index-detail-sort-l.html new file mode 100644 index 0000000000..39a15b4564 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-25 05:42:57Functions:204544.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
13.8%13.8%
+ 		13.8 %30 / 21831.8 %7 / 22
<unnamed>13.8 %30 / 21831.8 %7 / 22
batch_visualizer.cpp +
74.7%74.7%
+ 		74.7 %162 / 21756.5 %13 / 23
<unnamed>74.7 %162 / 21756.5 %13 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-detail.html b/mrs_lib/src/batch_visualizer/index-detail.html new file mode 100644 index 0000000000..ef8c49f1d7 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-25 05:42:57Functions:204544.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
batch_visualizer.cpp +
74.7%74.7%
+ 		74.7 %162 / 21756.5 %13 / 23
<unnamed>74.7 %162 / 21756.5 %13 / 23
visual_object.cpp +
13.8%13.8%
+ 		13.8 %30 / 21831.8 %7 / 22
<unnamed>13.8 %30 / 21831.8 %7 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-sort-f.html b/mrs_lib/src/batch_visualizer/index-sort-f.html new file mode 100644 index 0000000000..be77e2c6b2 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-25 05:42:57Functions:204544.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
13.8%13.8%
+ 		13.8 %30 / 21831.8 %7 / 22
batch_visualizer.cpp +
74.7%74.7%
+ 		74.7 %162 / 21756.5 %13 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-sort-l.html b/mrs_lib/src/batch_visualizer/index-sort-l.html new file mode 100644 index 0000000000..d07f0520a0 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-25 05:42:57Functions:204544.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
13.8%13.8%
+ 		13.8 %30 / 21831.8 %7 / 22
batch_visualizer.cpp +
74.7%74.7%
+ 		74.7 %162 / 21756.5 %13 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index.html b/mrs_lib/src/batch_visualizer/index.html new file mode 100644 index 0000000000..2bd5863454 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-25 05:42:57Functions:204544.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
batch_visualizer.cpp +
74.7%74.7%
+ 		74.7 %162 / 21756.5 %13 / 23
visual_object.cpp +
13.8%13.8%
+ 		13.8 %30 / 21831.8 %7 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html b/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html new file mode 100644 index 0000000000..a5a6b0dd2c --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - visual_object.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3021813.8 %
Date:2024-04-25 05:42:57Functions:72231.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::msgToEigen(geometry_msgs::Point_<std::allocator<void> > const&)0
mrs_lib::VisualObject::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int)0
mrs_lib::VisualObject::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool)0
mrs_lib::VisualObject::addRay(mrs_lib::geometry::Ray const&, double, double, double, double)0
mrs_lib::VisualObject::VisualObject(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_msgs::Path_<std::allocator<void> > const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cone const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cuboid const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ellipse const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cylinder const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Triangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Rectangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::buildEllipse(mrs_lib::geometry::Ellipse const&, int)0
mrs_lib::VisualObject::getID() const0
mrs_lib::VisualObject::isTimedOut() const56
mrs_lib::VisualObject::getType() const56
mrs_lib::VisualObject::getColors() const56
mrs_lib::VisualObject::getPoints() const56
mrs_lib::eigenToMsg(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)79
mrs_lib::VisualObject::VisualObject(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&, unsigned long const&)79
mrs_lib::generateColor(double, double, double, double)79
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.func.html b/mrs_lib/src/batch_visualizer/visual_object.cpp.func.html new file mode 100644 index 0000000000..39db6b55d5 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - visual_object.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3021813.8 %
Date:2024-04-25 05:42:57Functions:72231.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::eigenToMsg(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)79
mrs_lib::msgToEigen(geometry_msgs::Point_<std::allocator<void> > const&)0
mrs_lib::VisualObject::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int)0
mrs_lib::VisualObject::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool)0
mrs_lib::VisualObject::addRay(mrs_lib::geometry::Ray const&, double, double, double, double)0
mrs_lib::VisualObject::VisualObject(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&, unsigned long const&)79
mrs_lib::VisualObject::VisualObject(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_msgs::Path_<std::allocator<void> > const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cone const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cuboid const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ellipse const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cylinder const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Triangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Rectangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::buildEllipse(mrs_lib::geometry::Ellipse const&, int)0
mrs_lib::generateColor(double, double, double, double)79
mrs_lib::VisualObject::isTimedOut() const56
mrs_lib::VisualObject::getID() const0
mrs_lib::VisualObject::getType() const56
mrs_lib::VisualObject::getColors() const56
mrs_lib::VisualObject::getPoints() const56
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.frameset.html b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.frameset.html new file mode 100644 index 0000000000..fbafc2c961 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html new file mode 100644 index 0000000000..31b4b4c83f --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html @@ -0,0 +1,435 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - visual_object.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3021813.8 %
Date:2024-04-25 05:42:57Functions:72231.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/visual_object.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6             : /* utils //{ */
+       7             : 
+       8             : /* conversions //{ */
+       9          79 : geometry_msgs::Point eigenToMsg(const Eigen::Vector3d& v) {
+      10          79 :   geometry_msgs::Point p;
+      11          79 :   p.x = v.x();
+      12          79 :   p.y = v.y();
+      13          79 :   p.z = v.z();
+      14          79 :   return p;
+      15             : }
+      16             : 
+      17          79 : std_msgs::ColorRGBA generateColor(const double r, const double g, const double b, const double a) {
+      18          79 :   std_msgs::ColorRGBA c;
+      19          79 :   c.r = r;
+      20          79 :   c.g = g;
+      21          79 :   c.b = b;
+      22          79 :   c.a = a;
+      23          79 :   return c;
+      24             : }
+      25             : 
+      26           0 : Eigen::Vector3d msgToEigen(const geometry_msgs::Point& p) {
+      27           0 :   return Eigen::Vector3d(p.x, p.y, p.z);
+      28             : }
+      29             : //}
+      30             : 
+      31             : /* buildEllipse //{ */
+      32           0 : std::vector<Eigen::Vector3d> buildEllipse(const mrs_lib::geometry::Ellipse& ellipse, const int num_points) {
+      33           0 :   std::vector<Eigen::Vector3d> points;
+      34           0 :   double                       theta = 0;
+      35           0 :   for (int i = 0; i < num_points; i++) {
+      36           0 :     double          nom = (ellipse.a() * ellipse.b());
+      37           0 :     double          den = sqrt(((ellipse.b() * cos(theta)) * (ellipse.b() * cos(theta))) + ((ellipse.a() * sin(theta)) * (ellipse.a() * sin(theta))));
+      38           0 :     double          rho = nom / den;
+      39           0 :     Eigen::Vector3d point(rho * cos(theta), rho * sin(theta), 0);
+      40           0 :     point = ellipse.center() + ellipse.orientation() * point;
+      41           0 :     points.push_back(point);
+      42           0 :     theta += 2.0 * M_PI / num_points;
+      43             :   }
+      44           0 :   return points;
+      45             : }
+      46             : //}
+      47             : 
+      48             : //}
+      49             : 
+      50             : /* addRay //{ */
+      51           0 : void VisualObject::addRay(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a) {
+      52           0 :   type_ = MarkerType::LINE;
+      53           0 :   points_.push_back(eigenToMsg(ray.p1()));
+      54           0 :   points_.push_back(eigenToMsg(ray.p2()));
+      55           0 :   colors_.push_back(generateColor(r, g, b, a));
+      56           0 :   colors_.push_back(generateColor(r, g, b, a));
+      57           0 : }
+      58             : //}
+      59             : 
+      60             : /* addTriangle //{ */
+      61           0 : void VisualObject::addTriangle(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a, const bool filled) {
+      62           0 :   if (filled) {
+      63           0 :     type_ = MarkerType::TRIANGLE;
+      64           0 :     points_.push_back(eigenToMsg(triangle.a()));
+      65           0 :     points_.push_back(eigenToMsg(triangle.b()));
+      66           0 :     points_.push_back(eigenToMsg(triangle.c()));
+      67           0 :     colors_.push_back(generateColor(r, g, b, a));
+      68           0 :     colors_.push_back(generateColor(r, g, b, a));
+      69           0 :     colors_.push_back(generateColor(r, g, b, a));
+      70             :   } else {
+      71           0 :     type_ = MarkerType::LINE;
+      72           0 :     points_.push_back(eigenToMsg(triangle.a()));
+      73           0 :     points_.push_back(eigenToMsg(triangle.b()));
+      74           0 :     colors_.push_back(generateColor(r, g, b, a));
+      75           0 :     colors_.push_back(generateColor(r, g, b, a));
+      76             : 
+      77           0 :     points_.push_back(eigenToMsg(triangle.b()));
+      78           0 :     points_.push_back(eigenToMsg(triangle.c()));
+      79           0 :     colors_.push_back(generateColor(r, g, b, a));
+      80           0 :     colors_.push_back(generateColor(r, g, b, a));
+      81             : 
+      82           0 :     points_.push_back(eigenToMsg(triangle.c()));
+      83           0 :     points_.push_back(eigenToMsg(triangle.a()));
+      84           0 :     colors_.push_back(generateColor(r, g, b, a));
+      85           0 :     colors_.push_back(generateColor(r, g, b, a));
+      86             :   }
+      87           0 : }
+      88             : //}
+      89             : 
+      90             : /* addEllipse //{ */
+      91           0 : void VisualObject::addEllipse(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a, const bool filled,
+      92             :                               const int num_points) {
+      93             : 
+      94           0 :   std::vector<Eigen::Vector3d> points = buildEllipse(ellipse, num_points);
+      95           0 :   if (filled) {
+      96           0 :     for (int i = 0; i < num_points - 1; i++) {
+      97           0 :       mrs_lib::geometry::Triangle tri(ellipse.center(), points[i], points[i + 1]);
+      98           0 :       addTriangle(tri, r, g, b, a, true);
+      99             :     }
+     100           0 :     mrs_lib::geometry::Triangle tri(ellipse.center(), points[num_points - 1], points[0]);
+     101           0 :     addTriangle(tri, r, g, b, a, true);
+     102             : 
+     103             :   } else {
+     104           0 :     for (int i = 0; i < num_points - 1; i++) {
+     105           0 :       mrs_lib::geometry::Ray ray = mrs_lib::geometry::Ray::twopointCast(points[i], points[i + 1]);
+     106           0 :       addRay(ray, r, g, b, a);
+     107             :     }
+     108           0 :     mrs_lib::geometry::Ray ray = mrs_lib::geometry::Ray::twopointCast(points[num_points - 1], points[0]);
+     109           0 :     addRay(ray, r, g, b, a);
+     110             :   }
+     111           0 : }
+     112             : //}
+     113             : 
+     114             : /* Eigen::Vector3d //{ */
+     115          79 : VisualObject::VisualObject(const Eigen::Vector3d& point, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     116          79 :                            const unsigned long& id)
+     117          79 :     : id_(id) {
+     118          79 :   type_ = MarkerType::POINT;
+     119          79 :   points_.push_back(eigenToMsg(point));
+     120          79 :   colors_.push_back(generateColor(r, g, b, a));
+     121          79 :   if (timeout.toSec() <= 0) {
+     122          79 :     timeout_time_ = ros::Time(0);
+     123             :   } else {
+     124           0 :     timeout_time_ = ros::Time::now() + timeout;
+     125             :   }
+     126          79 : }
+     127             : //}
+     128             : 
+     129             : /* mrs_lib::geometry::Ray //{ */
+     130           0 : VisualObject::VisualObject(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     131           0 :                            const unsigned long& id)
+     132           0 :     : id_(id) {
+     133           0 :   type_ = MarkerType::LINE;
+     134           0 :   addRay(ray, r, g, b, a);
+     135           0 :   if (timeout.toSec() <= 0) {
+     136           0 :     timeout_time_ = ros::Time(0);
+     137             :   } else {
+     138           0 :     timeout_time_ = ros::Time::now() + timeout;
+     139             :   }
+     140           0 : }
+     141             : //}
+     142             : 
+     143             : /* mrs_lib::geometry::Triangle //{ */
+     144           0 : VisualObject::VisualObject(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a,
+     145           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     146           0 :     : id_(id) {
+     147           0 :   addTriangle(triangle, r, g, b, a, filled);
+     148           0 :   if (timeout.toSec() <= 0) {
+     149           0 :     timeout_time_ = ros::Time(0);
+     150             :   } else {
+     151           0 :     timeout_time_ = ros::Time::now() + timeout;
+     152             :   }
+     153           0 : }
+     154             : //}
+     155             : 
+     156             : /* mrs_lib::geometry::Rectangle //{ */
+     157           0 : VisualObject::VisualObject(const mrs_lib::geometry::Rectangle& rectangle, const double r, const double g, const double b, const double a,
+     158           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     159           0 :     : id_(id) {
+     160           0 :   for (const auto& t : rectangle.triangles()) {
+     161           0 :     addTriangle(t, r, g, b, a, filled);
+     162             :   }
+     163           0 :   if (timeout.toSec() <= 0) {
+     164           0 :     timeout_time_ = ros::Time(0);
+     165             :   } else {
+     166           0 :     timeout_time_ = ros::Time::now() + timeout;
+     167             :   }
+     168           0 : }
+     169             : //}
+     170             : 
+     171             : /* mrs_lib::geometry::Cuboid //{ */
+     172           0 : VisualObject::VisualObject(const mrs_lib::geometry::Cuboid& cuboid, const double r, const double g, const double b, const double a,
+     173           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     174           0 :     : id_(id) {
+     175             : 
+     176           0 :   for (int i = 0; i < 6; i++) {
+     177           0 :     for (const auto& t : cuboid.getRectangle(i).triangles()) {
+     178           0 :       addTriangle(t, r, g, b, a, filled);
+     179             :     }
+     180             :   }
+     181           0 :   if (timeout.toSec() <= 0) {
+     182           0 :     timeout_time_ = ros::Time(0);
+     183             :   } else {
+     184           0 :     timeout_time_ = ros::Time::now() + timeout;
+     185             :   }
+     186           0 : }
+     187             : //}
+     188             : 
+     189             : /* mrs_lib::geometry::Ellipse//{ */
+     190           0 : VisualObject::VisualObject(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a,
+     191           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id, const int num_points)
+     192           0 :     : id_(id) {
+     193           0 :   addEllipse(ellipse, r, g, b, a, filled, num_points);
+     194           0 :   if (timeout.toSec() <= 0) {
+     195           0 :     timeout_time_ = ros::Time(0);
+     196             :   } else {
+     197           0 :     timeout_time_ = ros::Time::now() + timeout;
+     198             :   }
+     199           0 : }
+     200             : //}
+     201             : 
+     202             : /* mrs_lib::geometry::Cylinder //{ */
+     203           0 : VisualObject::VisualObject(const mrs_lib::geometry::Cylinder& cylinder, const double r, const double g, const double b, const double a,
+     204           0 :                            const ros::Duration& timeout, const bool filled, const bool capped, const unsigned long& id, const int num_sides)
+     205           0 :     : id_(id) {
+     206           0 :   if (capped) {
+     207           0 :     mrs_lib::geometry::Ellipse top    = cylinder.getCap(mrs_lib::geometry::Cylinder::TOP);
+     208           0 :     mrs_lib::geometry::Ellipse bottom = cylinder.getCap(mrs_lib::geometry::Cylinder::BOTTOM);
+     209           0 :     addEllipse(top, r, g, b, a, filled, num_sides);
+     210           0 :     addEllipse(bottom, r, g, b, a, filled, num_sides);
+     211             :   }
+     212           0 :   std::vector<Eigen::Vector3d> top_points    = buildEllipse(cylinder.getCap(mrs_lib::geometry::Cylinder::TOP), num_sides);
+     213           0 :   std::vector<Eigen::Vector3d> bottom_points = buildEllipse(cylinder.getCap(mrs_lib::geometry::Cylinder::BOTTOM), num_sides);
+     214           0 :   for (unsigned int i = 0; i < top_points.size() - 1; i++) {
+     215           0 :     mrs_lib::geometry::Rectangle rect(bottom_points[i], bottom_points[i + 1], top_points[i + 1], top_points[i]);
+     216           0 :     addTriangle(rect.triangles()[0], r, g, b, a, filled);
+     217           0 :     addTriangle(rect.triangles()[1], r, g, b, a, filled);
+     218             :   }
+     219           0 :   mrs_lib::geometry::Rectangle rect(bottom_points[bottom_points.size() - 1], bottom_points[0], top_points[0], top_points[top_points.size() - 1]);
+     220           0 :   addTriangle(rect.triangles()[0], r, g, b, a, filled);
+     221           0 :   addTriangle(rect.triangles()[1], r, g, b, a, filled);
+     222           0 :   if (timeout.toSec() <= 0) {
+     223           0 :     timeout_time_ = ros::Time(0);
+     224             :   } else {
+     225           0 :     timeout_time_ = ros::Time::now() + timeout;
+     226             :   }
+     227           0 : }
+     228             : //}
+     229             : 
+     230             : /* mrs_lib::geometry::Cone //{ */
+     231           0 : VisualObject::VisualObject(const mrs_lib::geometry::Cone& cone, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     232           0 :                            const bool filled, const bool capped, const unsigned long& id, const int num_sides)
+     233           0 :     : id_(id) {
+     234           0 :   if (capped) {
+     235           0 :     mrs_lib::geometry::Ellipse cap = cone.getCap();
+     236           0 :     addEllipse(cap, r, g, b, a, filled, num_sides);
+     237             :   }
+     238           0 :   std::vector<Eigen::Vector3d> cap_points = buildEllipse(cone.getCap(), num_sides);
+     239           0 :   for (unsigned int i = 0; i < cap_points.size() - 1; i++) {
+     240           0 :     mrs_lib::geometry::Triangle tri(cap_points[i], cap_points[i + 1], cone.origin());
+     241           0 :     addTriangle(tri, r, g, b, a, filled);
+     242             :   }
+     243           0 :   mrs_lib::geometry::Triangle tri(cap_points[cap_points.size() - 1], cap_points[0], cone.origin());
+     244           0 :   addTriangle(tri, r, g, b, a, filled);
+     245           0 :   if (timeout.toSec() <= 0) {
+     246           0 :     timeout_time_ = ros::Time(0);
+     247             :   } else {
+     248           0 :     timeout_time_ = ros::Time::now() + timeout;
+     249             :   }
+     250           0 : }
+     251             : //}
+     252             : 
+     253             : /* mrs_msgs::Path //{ */
+     254           0 : VisualObject::VisualObject(const mrs_msgs::Path& p, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     255           0 :                            const bool filled, const unsigned long& id)
+     256           0 :     : id_(id) {
+     257           0 :   if (p.points.size() < 2) {
+     258           0 :     return;
+     259             :   }
+     260           0 :   if (filled) {
+     261           0 :     for (size_t i = 0; i < p.points.size() - 1; i++) {
+     262           0 :       Eigen::Vector3d p1, p2;
+     263           0 :       p1.x()   = p.points[i].position.x;
+     264           0 :       p1.y()   = p.points[i].position.y;
+     265           0 :       p1.z()   = p.points[i].position.z;
+     266           0 :       p2.x()   = p.points[i + 1].position.x;
+     267           0 :       p2.y()   = p.points[i + 1].position.y;
+     268           0 :       p2.z()   = p.points[i + 1].position.z;
+     269           0 :       auto ray = mrs_lib::geometry::Ray::twopointCast(p1, p2);
+     270           0 :       addRay(ray, r, g, b, a);
+     271             :     }
+     272             :   } else {
+     273           0 :     type_ = MarkerType::POINT;
+     274           0 :     for (size_t i = 0; i < p.points.size(); i++) {
+     275           0 :       points_.push_back(p.points[i].position);
+     276           0 :       colors_.push_back(generateColor(r, g, b, a));
+     277             :     }
+     278             :   }
+     279           0 :   if (timeout.toSec() <= 0) {
+     280           0 :     timeout_time_ = ros::Time(0);
+     281             :   } else {
+     282           0 :     timeout_time_ = ros::Time::now() + timeout;
+     283             :   }
+     284             : }
+     285             : //}
+     286             : 
+     287             : /* mrs_msgs::TrajectoryReference //{ */
+     288           0 : VisualObject::VisualObject(const mrs_msgs::TrajectoryReference& traj, const double r, const double g, const double b, const double a,
+     289           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     290           0 :     : id_(id) {
+     291           0 :   if (traj.points.size() < 2) {
+     292           0 :     return;
+     293             :   }
+     294           0 :   if (filled) {
+     295           0 :     for (size_t i = 0; i < traj.points.size() - 1; i++) {
+     296           0 :       Eigen::Vector3d p1, p2;
+     297           0 :       p1.x()   = traj.points[i].position.x;
+     298           0 :       p1.y()   = traj.points[i].position.y;
+     299           0 :       p1.z()   = traj.points[i].position.z;
+     300           0 :       p2.x()   = traj.points[i + 1].position.x;
+     301           0 :       p2.y()   = traj.points[i + 1].position.y;
+     302           0 :       p2.z()   = traj.points[i + 1].position.z;
+     303           0 :       auto ray = mrs_lib::geometry::Ray::twopointCast(p1, p2);
+     304           0 :       addRay(ray, r, g, b, a);
+     305             :     }
+     306             :   } else {
+     307           0 :     type_ = MarkerType::POINT;
+     308           0 :     for (size_t i = 0; i < traj.points.size(); i++) {
+     309           0 :       points_.push_back(traj.points[i].position);
+     310           0 :       colors_.push_back(generateColor(r, g, b, a));
+     311             :     }
+     312             :   }
+     313           0 :   if (timeout.toSec() <= 0) {
+     314           0 :     timeout_time_ = ros::Time(0);
+     315             :   } else {
+     316           0 :     timeout_time_ = ros::Time::now() + timeout;
+     317             :   }
+     318             : }
+     319             : //}
+     320             : 
+     321             : /* getID //{ */
+     322           0 : unsigned long VisualObject::getID() const {
+     323           0 :   return id_;
+     324             : }
+     325             : //}
+     326             : 
+     327             : /* getType //{ */
+     328          56 : int VisualObject::getType() const {
+     329          56 :   return type_;
+     330             : }
+     331             : //}
+     332             : 
+     333             : /* isTimedOut //{ */
+     334          56 : bool VisualObject::isTimedOut() const {
+     335          56 :   return !timeout_time_.isZero() && (timeout_time_ - ros::Time::now()).toSec() <= 0;
+     336             : }
+     337             : //}
+     338             : 
+     339             : /* getPoints //{ */
+     340          56 : const std::vector<geometry_msgs::Point> VisualObject::getPoints() const {
+     341          56 :   return points_;
+     342             : }
+     343             : //}
+     344             : 
+     345             : /* getColors //{ */
+     346          56 : const std::vector<std_msgs::ColorRGBA> VisualObject::getColors() const {
+     347          56 :   return colors_;
+     348             : }
+     349             : //}
+     350             : 
+     351             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html new file mode 100644 index 0000000000..a95610f629 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html @@ -0,0 +1,108 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b55f768965809edc613746889e563a4b1d7241f7 GIT binary patch literal 1229 zcmV;;1Ty=HP)2Nbs` zoTWIMm+F|wabY=fTua7yD)2OcZ?1tqZfc870j#l2Yw;o2ic`x%3rtvl0Lx4PG`AX> zSp(Y;p0(%cBnKv%DmNZ|4n&W5{FpRoIqT;Gxdu>ZXVGxGz%vc^5Lf8?!DL)P<*I@5 zT-jfvkf~5LBdgkHnks(V$ZzVIdDk+qANw=*M+J*P)A!J7s}Gn8z(w@s}=n{PM>>>MLb z;));`^*Izs%PdZxQL$rMD`g|Pj^LI9jWNKc=)j_ZG9A`thB>x`VVi`M2b^oV>-U>T zGyCMIic~Xi5$HML#Ir|^ZOT!OJ<8wY*MdCdS+#urW46DGJx)b|>OOhea=t2RHPAe} zbZcExHZN(OfCtu%?41D4X7mWGfr-zV8LDQ+!xo0F1!};|GwguD9;>>GNQR|GA)Zxe zj=lztn_2HAQhS>!&cOvW7FRqj}<;${}1n(02btrfqpw-A4y&R z)_Oi6NcBP$XFaKqYFJM|C~s-5<{Ge`M8~~gH>%3LjDiR5HHvmr;f=yw8mlLc|Q1EgcU0*vOlLx#mV%xQ`hWm90;EW<-=PckKX&-L<=B1XAnLSH;fvDBj9-*#5_Uj)e4#yy3u#JrPrkvOg~3Zg_6&wTMgM zj|Vo=%;SLnI^u44y0jH>$<0a#bVb~(M-kJfM_kcYPQ0lU_D{*(Z&&WKe)>Lp3ESD% zINWBB#s&iS4T$#chy)bxRXFPHD%k3lHo@?G2d?wYCjO-8U_PH~fwG^4vf~~gB! + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2024-04-25 05:42:57Functions:71070.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::toEigenMatrix(boost::array<double, 36ul> const&)0
mrs_lib::geometry::toCV(geometry_msgs::Vector3_<std::allocator<void> > const&)0
mrs_lib::geometry::fromEigen(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::geometry::toCV(geometry_msgs::Point_<std::allocator<void> > const&)1
mrs_lib::geometry::fromCV(cv::Point3_<double> const&)1
mrs_lib::geometry::toEigen(geometry_msgs::Vector3_<std::allocator<void> > const&)19
mrs_lib::geometry::fromEigenVec(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)22
mrs_lib::geometry::toEigen(geometry_msgs::Point_<std::allocator<void> > const&)28
mrs_lib::geometry::toEigen(geometry_msgs::Quaternion_<std::allocator<void> > const&)112766
mrs_lib::geometry::fromEigen(Eigen::Quaternion<double, 0> const&)112766
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/conversions.cpp.func.html b/mrs_lib/src/geometry/conversions.cpp.func.html new file mode 100644 index 0000000000..5dfe064822 --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2024-04-25 05:42:57Functions:71070.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::fromEigenVec(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)22
mrs_lib::geometry::toEigenMatrix(boost::array<double, 36ul> const&)0
mrs_lib::geometry::toCV(geometry_msgs::Point_<std::allocator<void> > const&)1
mrs_lib::geometry::toCV(geometry_msgs::Vector3_<std::allocator<void> > const&)0
mrs_lib::geometry::fromCV(cv::Point3_<double> const&)1
mrs_lib::geometry::toEigen(geometry_msgs::Quaternion_<std::allocator<void> > const&)112766
mrs_lib::geometry::toEigen(geometry_msgs::Point_<std::allocator<void> > const&)28
mrs_lib::geometry::toEigen(geometry_msgs::Vector3_<std::allocator<void> > const&)19
mrs_lib::geometry::fromEigen(Eigen::Quaternion<double, 0> const&)112766
mrs_lib::geometry::fromEigen(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/conversions.cpp.gcov.frameset.html b/mrs_lib/src/geometry/conversions.cpp.gcov.frameset.html new file mode 100644 index 0000000000..98a653461f --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/geometry/conversions.cpp.gcov.html b/mrs_lib/src/geometry/conversions.cpp.gcov.html new file mode 100644 index 0000000000..179bff24bb --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.gcov.html @@ -0,0 +1,178 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2024-04-25 05:42:57Functions:71070.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/geometry/conversions.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             :   namespace geometry
+       6             :   {
+       7             : 
+       8             :     /* conversions from/to Eigen //{ */
+       9             :     
+      10           0 :     geometry_msgs::Point fromEigen(const Eigen::Vector3d& what)
+      11             :     {
+      12           0 :       geometry_msgs::Point pt;
+      13           0 :       pt.x = what.x();
+      14           0 :       pt.y = what.y();
+      15           0 :       pt.z = what.z();
+      16           0 :       return pt;
+      17             :     }
+      18             :     
+      19          22 :     geometry_msgs::Vector3 fromEigenVec(const Eigen::Vector3d& what)
+      20             :     {
+      21          22 :       geometry_msgs::Vector3 pt;
+      22          22 :       pt.x = what.x();
+      23          22 :       pt.y = what.y();
+      24          22 :       pt.z = what.z();
+      25          22 :       return pt;
+      26             :     }
+      27             :     
+      28          28 :     Eigen::Vector3d toEigen(const geometry_msgs::Point& what)
+      29             :     {
+      30          28 :       return {what.x, what.y, what.z};
+      31             :     }
+      32             :     
+      33          19 :     Eigen::Vector3d toEigen(const geometry_msgs::Vector3& what)
+      34             :     {
+      35          19 :       return {what.x, what.y, what.z};
+      36             :     }
+      37             :     
+      38           0 :     Eigen::Matrix<double, 6, 6> toEigenMatrix(const boost::array<double, 36>& what)
+      39             :     {
+      40           0 :       Eigen::Matrix<double, 6, 6> ret;
+      41           0 :       for (int r = 0; r < 6; r++)
+      42           0 :         for (int c = 0; c < 6; c++)
+      43           0 :           ret(r, c) = what.at(6 * r + c);
+      44           0 :       return ret;
+      45             :     }
+      46             :     
+      47      112766 :     geometry_msgs::Quaternion fromEigen(const Eigen::Quaterniond& what)
+      48             :     {
+      49      112766 :       geometry_msgs::Quaternion q;
+      50      112764 :       q.x = what.x();
+      51      112766 :       q.y = what.y();
+      52      112764 :       q.z = what.z();
+      53      112765 :       q.w = what.w();
+      54      112765 :       return q;
+      55             :     }
+      56             :     
+      57      112766 :     Eigen::Quaterniond toEigen(const geometry_msgs::Quaternion& what)
+      58             :     {
+      59             :       // better to do this manually than through the constructor to avoid ambiguities (e.g. position of x and w)
+      60      112766 :       Eigen::Quaterniond q;
+      61      112765 :       q.x() = what.x;
+      62      112766 :       q.y() = what.y;
+      63      112766 :       q.z() = what.z;
+      64      112765 :       q.w() = what.w;
+      65      112765 :       return q;
+      66             :     }
+      67             :     
+      68             :     //}
+      69             : 
+      70             :     /* conversions from/to OpenCV //{ */
+      71             :     
+      72           1 :     geometry_msgs::Point fromCV(const cv::Point3d& what)
+      73             :     {
+      74           1 :       geometry_msgs::Point pt;
+      75           1 :       pt.x = what.x;
+      76           1 :       pt.y = what.y;
+      77           1 :       pt.z = what.z;
+      78           1 :       return pt;
+      79             :     }
+      80             :     
+      81           1 :     cv::Point3d toCV(const geometry_msgs::Point& what)
+      82             :     {
+      83           1 :       return {what.x, what.y, what.z};
+      84             :     }
+      85             :     
+      86           0 :     cv::Point3d toCV(const geometry_msgs::Vector3& what)
+      87             :     {
+      88           0 :       return {what.x, what.y, what.z};
+      89             :     }
+      90             :     
+      91             :     //}
+      92             : 
+      93             :   }
+      94             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/conversions.cpp.gcov.overview.html b/mrs_lib/src/geometry/conversions.cpp.gcov.overview.html new file mode 100644 index 0000000000..7d113fc895 --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.gcov.overview.html @@ -0,0 +1,44 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/geometry/conversions.cpp.gcov.png b/mrs_lib/src/geometry/conversions.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5a46c81f260e8d8a8c08cd829d3007493bf759f6 GIT binary patch literal 446 zcmV;v0YUzWP))a%5Q(iZDwZ>~SM>fqB$DL&a%jeJv#%>g3>p@_ zo0-zpqtyG3*)j2JoVnKE|plOZs#MMEH?+yM^cOu<+mC~a;5Dx+3g*S#0> z^CA>aIx+-?PGbnUxi>J&T`fd}X!zviLpk`X?KJFy9NQpkb^JY$`s&x};b6e$>1M_9r2&&rPm* o7DiIo6z(_7!W+s}%csSC0BpJVqo%=@)c^nh07*qoM6N<$g1hm+?EnA( literal 0 HcmV?d00001 diff --git a/mrs_lib/src/geometry/index-detail-sort-f.html b/mrs_lib/src/geometry/index-detail-sort-f.html new file mode 100644 index 0000000000..1c76168b92 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail-sort-f.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-25 05:42:57Functions:159416.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
54.7%54.7%
+ 		54.7 %35 / 6450.0 %8 / 16
<unnamed>54.7 %35 / 6450.0 %8 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
<unnamed>69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-detail-sort-l.html b/mrs_lib/src/geometry/index-detail-sort-l.html new file mode 100644 index 0000000000..bdf58d4807 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail-sort-l.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-25 05:42:57Functions:159416.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
54.7%54.7%
+ 		54.7 %35 / 6450.0 %8 / 16
<unnamed>54.7 %35 / 6450.0 %8 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
<unnamed>69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-detail.html b/mrs_lib/src/geometry/index-detail.html new file mode 100644 index 0000000000..719d4e7280 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-25 05:42:57Functions:159416.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
<unnamed>69.6 %32 / 4670.0 %7 / 10
misc.cpp +
54.7%54.7%
+ 		54.7 %35 / 6450.0 %8 / 16
<unnamed>54.7 %35 / 6450.0 %8 / 16
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-sort-f.html b/mrs_lib/src/geometry/index-sort-f.html new file mode 100644 index 0000000000..ba8b13f04a --- /dev/null +++ b/mrs_lib/src/geometry/index-sort-f.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-25 05:42:57Functions:159416.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
54.7%54.7%
+ 		54.7 %35 / 6450.0 %8 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-sort-l.html b/mrs_lib/src/geometry/index-sort-l.html new file mode 100644 index 0000000000..8dc6ee0f34 --- /dev/null +++ b/mrs_lib/src/geometry/index-sort-l.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-25 05:42:57Functions:159416.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
54.7%54.7%
+ 		54.7 %35 / 6450.0 %8 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index.html b/mrs_lib/src/geometry/index.html new file mode 100644 index 0000000000..460ec6f295 --- /dev/null +++ b/mrs_lib/src/geometry/index.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-25 05:42:57Functions:159416.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
misc.cpp +
54.7%54.7%
+ 		54.7 %35 / 6450.0 %8 / 16
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.func-sort-c.html b/mrs_lib/src/geometry/misc.cpp.func-sort-c.html new file mode 100644 index 0000000000..5be83379ea --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.func-sort-c.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-04-25 05:42:57Functions:81650.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::solidAngle(double, double, double)0
mrs_lib::geometry::invHaversin(double)0
mrs_lib::geometry::triangleArea(double, double, double)0
mrs_lib::geometry::quaternionBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)0
mrs_lib::geometry::quaternionFromEuler(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::geometry::quaternionFromEuler(double, double, double)0
mrs_lib::geometry::sphericalTriangleArea(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::haversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::rotationBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::angleaxisBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::cross(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1>)3
mrs_lib::geometry::quaternionFromHeading(double)112772
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)140030
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)208288
mrs_lib::geometry::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)374480
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.func.html b/mrs_lib/src/geometry/misc.cpp.func.html new file mode 100644 index 0000000000..3a9a0c3b42 --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-04-25 05:42:57Functions:81650.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::solidAngle(double, double, double)0
mrs_lib::geometry::invHaversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)140030
mrs_lib::geometry::triangleArea(double, double, double)0
mrs_lib::geometry::rotationBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::angleaxisBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::quaternionBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)0
mrs_lib::geometry::quaternionFromEuler(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::geometry::quaternionFromEuler(double, double, double)0
mrs_lib::geometry::quaternionFromHeading(double)112772
mrs_lib::geometry::sphericalTriangleArea(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)374480
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)208288
mrs_lib::geometry::cross(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1>)3
mrs_lib::geometry::haversin(double)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.frameset.html b/mrs_lib/src/geometry/misc.cpp.gcov.frameset.html new file mode 100644 index 0000000000..35b80de4af --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.html b/mrs_lib/src/geometry/misc.cpp.gcov.html new file mode 100644 index 0000000000..ec6a292871 --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.html @@ -0,0 +1,269 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-04-25 05:42:57Functions:81650.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : #include <mrs_lib/geometry/misc.h>
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             :   namespace geometry
+       7             :   {
+       8             : 
+       9             :     // instantiation of common template values
+      10             :     vec_t<3 + 1> toHomogenous(const vec_t<3>& vec);
+      11             :     vec_t<2 + 1> toHomogenous(const vec_t<2>& vec);
+      12             : 
+      13             :     // | ----------------- Angle-related functions ---------------- |
+      14             : 
+      15             :     /* angle-related functions //{ */
+      16             : 
+      17             :     /* cross() //{ */
+      18             : 
+      19           3 :     double cross(const vec2_t& vec1, const vec2_t vec2)
+      20             :     {
+      21           3 :       return vec1.x() * vec2.y() - vec1.y() * vec2.x();
+      22             :     }
+      23             : 
+      24             :     //}
+      25             : 
+      26             :     /* angleBetween() //{ */
+      27             : 
+      28      140030 :     double angleBetween(const vec3_t& vec1, const vec3_t& vec2)
+      29             :     {
+      30      140030 :       const double sin_12 = vec1.cross(vec2).norm();
+      31      140024 :       const double cos_12 = vec1.dot(vec2);
+      32      140015 :       const double angle = std::atan2(sin_12, cos_12);
+      33      140015 :       return angle;
+      34             :     }
+      35             : 
+      36           3 :     double angleBetween(const vec2_t& vec1, const vec2_t& vec2)
+      37             :     {
+      38           3 :       const double sin_12 = cross(vec1, vec2);
+      39           3 :       const double cos_12 = vec1.dot(vec2);
+      40           3 :       const double angle = std::atan2(sin_12, cos_12);
+      41           3 :       return angle;
+      42             :     }
+      43             : 
+      44             :     //}
+      45             : 
+      46             :     /* angleaxisBetween() //{ */
+      47             : 
+      48           3 :     anax_t angleaxisBetween(const Eigen::Vector3d& vec1, const Eigen::Vector3d& vec2, const double tolerance)
+      49             :     {
+      50             :       // Find the rotation matrix to rotate vec1 to point in the direction of vec2
+      51           3 :       const Eigen::Vector3d a = vec1.normalized();
+      52           3 :       const Eigen::Vector3d b = vec2.normalized();
+      53           3 :       const Eigen::Vector3d v = a.cross(b);
+      54           3 :       const double sin_ab = v.norm();
+      55           3 :       const double cos_ab = a.dot(b);
+      56           3 :       const double angle = std::atan2(sin_ab, cos_ab);
+      57           3 :       anax_t ret;
+      58           3 :       if (abs(angle) < tolerance)
+      59           1 :         ret = anax_t(0.0, Eigen::Vector3d::UnitX());
+      60           2 :       else if (abs(abs(angle) - M_PI) < tolerance)
+      61           0 :         ret = anax_t(M_PI, Eigen::Vector3d::UnitX());
+      62             :       else
+      63           2 :         ret = anax_t(angle, v.normalized());
+      64           6 :       return ret;
+      65             :     }
+      66             : 
+      67             :     //}
+      68             : 
+      69             :     /* quaternionBetween() //{ */
+      70             : 
+      71           0 :     quat_t quaternionBetween(const Eigen::Vector3d& vec1, const Eigen::Vector3d& vec2, const double tolerance)
+      72             :     {
+      73           0 :       const auto rot = angleaxisBetween(vec1, vec2, tolerance);
+      74           0 :       const quat_t ret(rot);
+      75           0 :       return ret;
+      76             :     }
+      77             : 
+      78             :     /* quaternionFromEuler() overloads //{ */
+      79           0 :     quat_t quaternionFromEuler(double x, double y, double z)
+      80             :     {
+      81           0 :       return anax_t(x, vec3_t::UnitX()) * anax_t(y, vec3_t::UnitY()) * anax_t(z, vec3_t::UnitZ());
+      82             :     }
+      83             : 
+      84           0 :     quat_t quaternionFromEuler(const Eigen::Vector3d& euler)
+      85             :     {
+      86           0 :       return anax_t(euler.x(), vec3_t::UnitX()) * anax_t(euler.y(), vec3_t::UnitY())
+      87           0 :              * anax_t(euler.z(), vec3_t::UnitZ());
+      88             :     }
+      89             :     //}
+      90             : 
+      91             :     /* quaternionFromHeading //{ */
+      92      112772 :     quat_t quaternionFromHeading(const double heading)
+      93             :     {
+      94      225542 :       return quat_t(anax_t(heading, Eigen::Vector3d::UnitZ()));
+      95             :     }
+      96             :     //}
+      97             : 
+      98             :     //}
+      99             : 
+     100             :     /* rotationBetween() //{ */
+     101             : 
+     102           3 :     Eigen::Matrix3d rotationBetween(const Eigen::Vector3d& vec1, const Eigen::Vector3d& vec2, const double tolerance)
+     103             :     {
+     104           3 :       const auto rot = angleaxisBetween(vec1, vec2, tolerance);
+     105           3 :       const Eigen::Matrix3d ret(rot);
+     106           6 :       return ret;
+     107             :     }
+     108             : 
+     109             :     //}
+     110             : 
+     111             :     /* haversin() //{ */
+     112             : 
+     113           0 :     double haversin(const double angle)
+     114             :     {
+     115           0 :       return (1.0 - std::cos(angle)) / 2.0;
+     116             :     }
+     117             : 
+     118             :     //}
+     119             : 
+     120             :     /* invHaversin() //{ */
+     121             : 
+     122           0 :     double invHaversin(const double value)
+     123             :     {
+     124           0 :       return 2.0 * std::asin(std::sqrt(value));
+     125             :     }
+     126             : 
+     127             :     //}
+     128             : 
+     129             :     /* solidAngle() //{ */
+     130           0 :     double solidAngle(double a, double b, double c)
+     131             :     {
+     132           0 :       return invHaversin((haversin(c) - haversin(a - b)) / (std::sin(a) * std::sin(b)));
+     133             :     }
+     134             :     //}
+     135             : 
+     136             :     //}
+     137             : 
+     138             :     /* triangleArea() //{ */
+     139             : 
+     140           0 :     double triangleArea(const double a, const double b, const double c)
+     141             :     {
+     142           0 :       double s = (a + b + c) / 2.0;
+     143           0 :       return std::sqrt(s * (s - a) * (s - b) * (s - c));
+     144             :     }
+     145             : 
+     146             :     //}
+     147             : 
+     148             :     /* sphericalTriangleArea //{ */
+     149           0 :     double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c)
+     150             :     {
+     151           0 :       double ab = angleBetween(a, b);
+     152           0 :       double bc = angleBetween(b, c);
+     153           0 :       double ca = angleBetween(c, a);
+     154             : 
+     155           0 :       if (ab < 1e-3 and bc < 1e-3 and ca < 1e-3)
+     156             :       {
+     157           0 :         return triangleArea(ab, bc, ca);
+     158             :       }
+     159             : 
+     160           0 :       double A = solidAngle(ca, ab, bc);
+     161           0 :       double B = solidAngle(ab, bc, ca);
+     162           0 :       double C = solidAngle(bc, ca, ab);
+     163             : 
+     164           0 :       return A + B + C - M_PI;
+     165             :     }
+     166             :     //}
+     167             : 
+     168             :     /* vector distance //{ */
+     169             : 
+     170      374480 :     double dist(const vec2_t& a, const vec2_t& b)
+     171             :     {
+     172             : 
+     173      374480 :       return (a - b).norm();
+     174             :     }
+     175             : 
+     176      208288 :     double dist(const vec3_t& a, const vec3_t& b)
+     177             :     {
+     178             : 
+     179      208288 :       return (a - b).norm();
+     180             :     }
+     181             : 
+     182             :     //}
+     183             : 
+     184             :   }  // namespace geometry
+     185             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.overview.html b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html new file mode 100644 index 0000000000..bdcd7dc00e --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html @@ -0,0 +1,67 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.png b/mrs_lib/src/geometry/misc.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ae4a5f86166101e9403d16475182cfe6bae4cdaf GIT binary patch literal 747 zcmVX!Z#y-%&@+7LS{G9BH$opGoGF?W z)&8!N@Lg-wJX@&&W(Em>TVN~80F}C;jiY`2Ej;cCi5I~4@;EbQMnz5NYr`PJ=}JFt zqDL4>b=6QsURQcqp5#3-waU9t4S>Sy($`(U((1|N&xgRO^;lw40ZsMTt_Rk@6-Skq zoQ<33$Aws|@epzXoJ#nK1P&|`c%DaD0vo-NS3K6y^R93m(^*Kq1CQ8yu!j}_gtA+l z3qdO4NUzfc2~ZDN(kM_R1L|$;BmAy4On4MC4eavJ{r#xWV}9FW<50u@4_&yh0M0=X%260*yHjVmZp|$Hi4drgpb)zDHdds58TAGtN#T&DuOV~=Zi1g{|f#W}!w8&zP!3FwmKQVFb@w%%XkE* + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - shapes.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2024-04-25 05:42:57Functions:0680.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::Ray::twopointCast(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Ray::directionCast(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Ray::Ray(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Ray::Ray()0
mrs_lib::geometry::Ray::~Ray()0
mrs_lib::geometry::Cone::Cone(Eigen::Matrix<double, 3, 1, 0, 3, 1>, double, double, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Cone::Cone()0
mrs_lib::geometry::Cone::~Cone()0
mrs_lib::geometry::Cuboid::Cuboid(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Quaternion<double, 0>)0
mrs_lib::geometry::Cuboid::Cuboid(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Cuboid::Cuboid(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >)0
mrs_lib::geometry::Cuboid::Cuboid()0
mrs_lib::geometry::Cuboid::~Cuboid()0
mrs_lib::geometry::Ellipse::Ellipse(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Quaternion<double, 0>, double, double)0
mrs_lib::geometry::Ellipse::Ellipse()0
mrs_lib::geometry::Ellipse::~Ellipse()0
mrs_lib::geometry::Cylinder::Cylinder(Eigen::Matrix<double, 3, 1, 0, 3, 1>, double, double, Eigen::Quaternion<double, 0>)0
mrs_lib::geometry::Cylinder::Cylinder()0
mrs_lib::geometry::Cylinder::~Cylinder()0
mrs_lib::geometry::Triangle::Triangle(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Triangle::Triangle()0
mrs_lib::geometry::Triangle::~Triangle()0
mrs_lib::geometry::Rectangle::Rectangle(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Rectangle::Rectangle(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >)0
mrs_lib::geometry::Rectangle::Rectangle()0
mrs_lib::geometry::Rectangle::~Rectangle()0
mrs_lib::geometry::Ray::p1() const0
mrs_lib::geometry::Ray::p2() const0
mrs_lib::geometry::Ray::direction() const0
mrs_lib::geometry::Cone::projectPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&) const0
mrs_lib::geometry::Cone::h() const0
mrs_lib::geometry::Cone::theta() const0
mrs_lib::geometry::Cone::center() const0
mrs_lib::geometry::Cone::getCap() const0
mrs_lib::geometry::Cone::origin() const0
mrs_lib::geometry::Cone::direction() const0
mrs_lib::geometry::Cuboid::getRectangle(int) const0
mrs_lib::geometry::Cuboid::lookupPoints(int) const0
mrs_lib::geometry::Cuboid::intersectionRay(mrs_lib::geometry::Ray, double) const0
mrs_lib::geometry::Cuboid::center() const0
mrs_lib::geometry::Cuboid::vertices() const0
mrs_lib::geometry::Ellipse::orientation() const0
mrs_lib::geometry::Ellipse::a() const0
mrs_lib::geometry::Ellipse::b() const0
mrs_lib::geometry::Ellipse::center() const0
mrs_lib::geometry::Cylinder::orientation() const0
mrs_lib::geometry::Cylinder::h() const0
mrs_lib::geometry::Cylinder::r() const0
mrs_lib::geometry::Cylinder::center() const0
mrs_lib::geometry::Cylinder::getCap(int) const0
mrs_lib::geometry::Triangle::intersectionRay(mrs_lib::geometry::Ray, double) const0
mrs_lib::geometry::Triangle::a() const0
mrs_lib::geometry::Triangle::b() const0
mrs_lib::geometry::Triangle::c() const0
mrs_lib::geometry::Triangle::center() const0
mrs_lib::geometry::Triangle::normal() const0
mrs_lib::geometry::Triangle::vertices() const0
mrs_lib::geometry::Rectangle::intersectionRay(mrs_lib::geometry::Ray, double) const0
mrs_lib::geometry::Rectangle::a() const0
mrs_lib::geometry::Rectangle::b() const0
mrs_lib::geometry::Rectangle::c() const0
mrs_lib::geometry::Rectangle::d() const0
mrs_lib::geometry::Rectangle::solidAngleRelativeTo(Eigen::Matrix<double, 3, 1, 0, 3, 1>) const0
mrs_lib::geometry::Rectangle::center() const0
mrs_lib::geometry::Rectangle::normal() const0
mrs_lib::geometry::Rectangle::isFacing(Eigen::Matrix<double, 3, 1, 0, 3, 1>) const0
mrs_lib::geometry::Rectangle::vertices() const0
mrs_lib::geometry::Rectangle::triangles() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/shapes.cpp.func.html b/mrs_lib/src/geometry/shapes.cpp.func.html new file mode 100644 index 0000000000..82c0ed16da --- /dev/null +++ b/mrs_lib/src/geometry/shapes.cpp.func.html @@ -0,0 +1,352 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - shapes.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2024-04-25 05:42:57Functions:0680.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::Ray::twopointCast(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Ray::directionCast(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Ray::Ray(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Ray::Ray()0
mrs_lib::geometry::Ray::~Ray()0
mrs_lib::geometry::Cone::Cone(Eigen::Matrix<double, 3, 1, 0, 3, 1>, double, double, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Cone::Cone()0
mrs_lib::geometry::Cone::~Cone()0
mrs_lib::geometry::Cuboid::Cuboid(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Quaternion<double, 0>)0
mrs_lib::geometry::Cuboid::Cuboid(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Cuboid::Cuboid(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >)0
mrs_lib::geometry::Cuboid::Cuboid()0
mrs_lib::geometry::Cuboid::~Cuboid()0
mrs_lib::geometry::Ellipse::Ellipse(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Quaternion<double, 0>, double, double)0
mrs_lib::geometry::Ellipse::Ellipse()0
mrs_lib::geometry::Ellipse::~Ellipse()0
mrs_lib::geometry::Cylinder::Cylinder(Eigen::Matrix<double, 3, 1, 0, 3, 1>, double, double, Eigen::Quaternion<double, 0>)0
mrs_lib::geometry::Cylinder::Cylinder()0
mrs_lib::geometry::Cylinder::~Cylinder()0
mrs_lib::geometry::Triangle::Triangle(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Triangle::Triangle()0
mrs_lib::geometry::Triangle::~Triangle()0
mrs_lib::geometry::Rectangle::Rectangle(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::Rectangle::Rectangle(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >)0
mrs_lib::geometry::Rectangle::Rectangle()0
mrs_lib::geometry::Rectangle::~Rectangle()0
mrs_lib::geometry::Ray::p1() const0
mrs_lib::geometry::Ray::p2() const0
mrs_lib::geometry::Ray::direction() const0
mrs_lib::geometry::Cone::projectPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&) const0
mrs_lib::geometry::Cone::h() const0
mrs_lib::geometry::Cone::theta() const0
mrs_lib::geometry::Cone::center() const0
mrs_lib::geometry::Cone::getCap() const0
mrs_lib::geometry::Cone::origin() const0
mrs_lib::geometry::Cone::direction() const0
mrs_lib::geometry::Cuboid::getRectangle(int) const0
mrs_lib::geometry::Cuboid::lookupPoints(int) const0
mrs_lib::geometry::Cuboid::intersectionRay(mrs_lib::geometry::Ray, double) const0
mrs_lib::geometry::Cuboid::center() const0
mrs_lib::geometry::Cuboid::vertices() const0
mrs_lib::geometry::Ellipse::orientation() const0
mrs_lib::geometry::Ellipse::a() const0
mrs_lib::geometry::Ellipse::b() const0
mrs_lib::geometry::Ellipse::center() const0
mrs_lib::geometry::Cylinder::orientation() const0
mrs_lib::geometry::Cylinder::h() const0
mrs_lib::geometry::Cylinder::r() const0
mrs_lib::geometry::Cylinder::center() const0
mrs_lib::geometry::Cylinder::getCap(int) const0
mrs_lib::geometry::Triangle::intersectionRay(mrs_lib::geometry::Ray, double) const0
mrs_lib::geometry::Triangle::a() const0
mrs_lib::geometry::Triangle::b() const0
mrs_lib::geometry::Triangle::c() const0
mrs_lib::geometry::Triangle::center() const0
mrs_lib::geometry::Triangle::normal() const0
mrs_lib::geometry::Triangle::vertices() const0
mrs_lib::geometry::Rectangle::intersectionRay(mrs_lib::geometry::Ray, double) const0
mrs_lib::geometry::Rectangle::a() const0
mrs_lib::geometry::Rectangle::b() const0
mrs_lib::geometry::Rectangle::c() const0
mrs_lib::geometry::Rectangle::d() const0
mrs_lib::geometry::Rectangle::solidAngleRelativeTo(Eigen::Matrix<double, 3, 1, 0, 3, 1>) const0
mrs_lib::geometry::Rectangle::center() const0
mrs_lib::geometry::Rectangle::normal() const0
mrs_lib::geometry::Rectangle::isFacing(Eigen::Matrix<double, 3, 1, 0, 3, 1>) const0
mrs_lib::geometry::Rectangle::vertices() const0
mrs_lib::geometry::Rectangle::triangles() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/shapes.cpp.gcov.frameset.html b/mrs_lib/src/geometry/shapes.cpp.gcov.frameset.html new file mode 100644 index 0000000000..d1dafd1fc9 --- /dev/null +++ b/mrs_lib/src/geometry/shapes.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/geometry/shapes.cpp.gcov.html b/mrs_lib/src/geometry/shapes.cpp.gcov.html new file mode 100644 index 0000000000..4302941bf2 --- /dev/null +++ b/mrs_lib/src/geometry/shapes.cpp.gcov.html @@ -0,0 +1,730 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - shapes.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2024-04-25 05:42:57Functions:0680.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : #include <mrs_lib/geometry/shapes.h>
+       3             : #include <mrs_lib/geometry/misc.h>
+       4             : 
+       5             : namespace mrs_lib
+       6             : {
+       7             :   namespace geometry
+       8             :   {
+       9             : 
+      10             :     /* Ray //{ */
+      11             : 
+      12             :     /* constructors //{ */
+      13           0 :     Ray::Ray()
+      14             :     {
+      15           0 :       point1 = Eigen::Vector3d::Zero();
+      16           0 :       point2 = Eigen::Vector3d::Zero();
+      17           0 :     }
+      18             : 
+      19           0 :     Ray::Ray(Eigen::Vector3d p1, Eigen::Vector3d p2)
+      20             :     {
+      21           0 :       point1 = p1;
+      22           0 :       point2 = p2;
+      23           0 :     }
+      24             : 
+      25           0 :     Ray::~Ray()
+      26             :     {
+      27           0 :     }
+      28             :     //}
+      29             : 
+      30             :     /* getters //{ */
+      31           0 :     const Eigen::Vector3d Ray::p1() const
+      32             :     {
+      33           0 :       return point1;
+      34             :     }
+      35             : 
+      36           0 :     const Eigen::Vector3d Ray::p2() const
+      37             :     {
+      38           0 :       return point2;
+      39             :     }
+      40             : 
+      41           0 :     const Eigen::Vector3d Ray::direction() const
+      42             :     {
+      43           0 :       return (point2 - point1);
+      44             :     }
+      45             :     //}
+      46             : 
+      47             :     /* raycasting //{ */
+      48           0 :     Ray Ray::twopointCast(Eigen::Vector3d pointFrom, Eigen::Vector3d pointTo)
+      49             :     {
+      50           0 :       return Ray(pointFrom, pointTo);
+      51             :     }
+      52             : 
+      53           0 :     Ray Ray::directionCast(Eigen::Vector3d origin, Eigen::Vector3d direction)
+      54             :     {
+      55           0 :       return Ray(origin, origin + direction);
+      56             :     }
+      57             :     //}
+      58             : 
+      59             :     //}
+      60             : 
+      61             :     /* Triangle //{ */
+      62             : 
+      63             :     /* constructors //{ */
+      64           0 :     Triangle::Triangle()
+      65             :     {
+      66           0 :       point1 = Eigen::Vector3d(0, 0, 0);
+      67           0 :       point2 = Eigen::Vector3d(1, 0, 0);
+      68           0 :       point3 = Eigen::Vector3d(0, 0, 1);
+      69           0 :     }
+      70             : 
+      71           0 :     Triangle::Triangle(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c)
+      72             :     {
+      73           0 :       point1 = a;
+      74           0 :       point2 = b;
+      75           0 :       point3 = c;
+      76           0 :     }
+      77             : 
+      78           0 :     Triangle::~Triangle()
+      79             :     {
+      80           0 :     }
+      81             :     //}
+      82             : 
+      83             :     /* getters //{ */
+      84           0 :     const Eigen::Vector3d Triangle::a() const
+      85             :     {
+      86           0 :       return point1;
+      87             :     }
+      88             : 
+      89           0 :     const Eigen::Vector3d Triangle::b() const
+      90             :     {
+      91           0 :       return point2;
+      92             :     }
+      93             : 
+      94           0 :     const Eigen::Vector3d Triangle::c() const
+      95             :     {
+      96           0 :       return point3;
+      97             :     }
+      98             : 
+      99           0 :     const Eigen::Vector3d Triangle::normal() const
+     100             :     {
+     101           0 :       Eigen::Vector3d n;
+     102           0 :       n = (point2 - point1).cross(point3 - point1);
+     103           0 :       return n.normalized();
+     104             :     }
+     105             : 
+     106           0 :     const Eigen::Vector3d Triangle::center() const
+     107             :     {
+     108           0 :       return (point1 + point2 + point3) / 3.0;
+     109             :     }
+     110             : 
+     111           0 :     const std::vector<Eigen::Vector3d> Triangle::vertices() const
+     112             :     {
+     113           0 :       std::vector<Eigen::Vector3d> vertices;
+     114           0 :       vertices.push_back(point1);
+     115           0 :       vertices.push_back(point2);
+     116           0 :       vertices.push_back(point3);
+     117           0 :       return vertices;
+     118             :     }
+     119             :     //}
+     120             : 
+     121             :     /* intersectionRay //{ */
+     122           0 :     const boost::optional<Eigen::Vector3d> Triangle::intersectionRay(Ray r, double epsilon) const
+     123             :     {
+     124             :       // The Möller–Trumbore algorithm
+     125             :       // https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
+     126           0 :       Eigen::Vector3d v1 = point2 - point1;
+     127           0 :       Eigen::Vector3d v2 = point3 - point1;
+     128           0 :       Eigen::Vector3d h = r.direction().cross(v2);
+     129           0 :       double res = v1.dot(h);
+     130           0 :       if (res > -epsilon && res < epsilon)
+     131             :       {
+     132           0 :         return boost::none;
+     133             :       }
+     134           0 :       double f = 1.0 / res;
+     135           0 :       Eigen::Vector3d s = r.p1() - point1;
+     136           0 :       double u = f * s.dot(h);
+     137           0 :       if (u < 0.0 || u > 1.0)
+     138             :       {
+     139           0 :         return boost::none;
+     140             :       }
+     141           0 :       Eigen::Vector3d q = s.cross(v1);
+     142           0 :       double v = f * r.direction().dot(q);
+     143           0 :       if (v < 0.0 || u + v > 1.0)
+     144             :       {
+     145           0 :         return boost::none;
+     146             :       }
+     147           0 :       double t = f * v2.dot(q);
+     148           0 :       if (t > epsilon)
+     149             :       {
+     150           0 :         Eigen::Vector3d ret = r.p1() + r.direction() * t;
+     151           0 :         return ret;
+     152             :       }
+     153           0 :       return boost::none;
+     154             :     }
+     155             :     //}
+     156             : 
+     157             :     //}
+     158             : 
+     159             :     /* Rectangle //{ */
+     160             : 
+     161             :     /* constructors //{ */
+     162           0 :     Rectangle::Rectangle()
+     163             :     {
+     164           0 :       point1 = Eigen::Vector3d(0, 0, 0);
+     165           0 :       point2 = Eigen::Vector3d(1, 0, 0);
+     166           0 :       point3 = Eigen::Vector3d(1, 1, 0);
+     167           0 :       point4 = Eigen::Vector3d(0, 1, 0);
+     168           0 :     }
+     169             : 
+     170           0 :     Rectangle::Rectangle(std::vector<Eigen::Vector3d> points)
+     171             :     {
+     172           0 :       point1 = points[0];
+     173           0 :       point2 = points[1];
+     174           0 :       point3 = points[2];
+     175           0 :       point4 = points[3];
+     176           0 :     }
+     177             : 
+     178           0 :     Rectangle::Rectangle(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c, Eigen::Vector3d d)
+     179             :     {
+     180           0 :       point1 = a;
+     181           0 :       point2 = b;
+     182           0 :       point3 = c;
+     183           0 :       point4 = d;
+     184           0 :     }
+     185             : 
+     186           0 :     Rectangle::~Rectangle()
+     187             :     {
+     188           0 :     }
+     189             :     //}
+     190             : 
+     191             :     /* getters //{ */
+     192           0 :     const Eigen::Vector3d Rectangle::a() const
+     193             :     {
+     194           0 :       return point1;
+     195             :     }
+     196             : 
+     197           0 :     const Eigen::Vector3d Rectangle::b() const
+     198             :     {
+     199           0 :       return point2;
+     200             :     }
+     201             : 
+     202           0 :     const Eigen::Vector3d Rectangle::c() const
+     203             :     {
+     204           0 :       return point3;
+     205             :     }
+     206             : 
+     207           0 :     const Eigen::Vector3d Rectangle::d() const
+     208             :     {
+     209           0 :       return point4;
+     210             :     }
+     211             : 
+     212           0 :     const Eigen::Vector3d Rectangle::center() const
+     213             :     {
+     214           0 :       return (point1 + point2 + point3 + point4) / 4.0;
+     215             :     }
+     216             : 
+     217           0 :     const Eigen::Vector3d Rectangle::normal() const
+     218             :     {
+     219           0 :       Eigen::Vector3d n;
+     220           0 :       n = (point2 - point1).cross(point4 - point1);
+     221           0 :       return n.normalized();
+     222             :     }
+     223             : 
+     224           0 :     const std::vector<Eigen::Vector3d> Rectangle::vertices() const
+     225             :     {
+     226           0 :       std::vector<Eigen::Vector3d> vertices;
+     227           0 :       vertices.push_back(point1);
+     228           0 :       vertices.push_back(point2);
+     229           0 :       vertices.push_back(point3);
+     230           0 :       vertices.push_back(point4);
+     231           0 :       return vertices;
+     232             :     }
+     233             : 
+     234           0 :     const std::vector<Triangle> Rectangle::triangles() const
+     235             :     {
+     236           0 :       Triangle t1(point1, point2, point3);
+     237           0 :       Triangle t2(point1, point3, point4);
+     238             : 
+     239           0 :       std::vector<Triangle> triangles;
+     240           0 :       triangles.push_back(t1);
+     241           0 :       triangles.push_back(t2);
+     242           0 :       return triangles;
+     243             :     }
+     244             :     //}
+     245             : 
+     246             :     /* intersectionRay //{ */
+     247           0 :     const boost::optional<Eigen::Vector3d> Rectangle::intersectionRay(Ray r, double epsilon) const
+     248             :     {
+     249           0 :       Triangle t1 = triangles()[0];
+     250           0 :       Triangle t2 = triangles()[1];
+     251           0 :       auto result = t1.intersectionRay(r, epsilon);
+     252           0 :       if (result != boost::none)
+     253             :       {
+     254           0 :         return result;
+     255             :       }
+     256           0 :       return t2.intersectionRay(r, epsilon);
+     257             :     }
+     258             :     //}
+     259             : 
+     260             :     /* isFacing //{ */
+     261           0 :     bool Rectangle::isFacing(Eigen::Vector3d point) const
+     262             :     {
+     263           0 :       Eigen::Vector3d towards_point = point - center();
+     264           0 :       double dot_product = towards_point.dot(normal());
+     265           0 :       return dot_product > 0;
+     266             :     }
+     267             : 
+     268             :     //}
+     269             : 
+     270             :     /* solidAngleRelativeTo //{ */
+     271           0 :     double Rectangle::solidAngleRelativeTo(Eigen::Vector3d point) const
+     272             :     {
+     273           0 :       Eigen::Vector3d a = point1 - point;
+     274           0 :       Eigen::Vector3d b = point2 - point;
+     275           0 :       Eigen::Vector3d c = point3 - point;
+     276           0 :       Eigen::Vector3d d = point4 - point;
+     277             : 
+     278           0 :       a.normalize();
+     279           0 :       b.normalize();
+     280           0 :       c.normalize();
+     281           0 :       d.normalize();
+     282             : 
+     283           0 :       double t1 = mrs_lib::geometry::sphericalTriangleArea(a, b, c);
+     284           0 :       double t2 = mrs_lib::geometry::sphericalTriangleArea(c, d, a);
+     285             : 
+     286           0 :       return t1 + t2;
+     287             :     }
+     288             :     //}
+     289             : 
+     290             :     //}
+     291             : 
+     292             :     /* Cuboid //{ */
+     293             : 
+     294             :     /* constructors //{ */
+     295           0 :     Cuboid::Cuboid()
+     296             :     {
+     297           0 :       for (int i = 0; i < 8; i++)
+     298             :       {
+     299           0 :         points.push_back(Eigen::Vector3d::Zero());
+     300             :       }
+     301           0 :     }
+     302             : 
+     303           0 :     Cuboid::Cuboid(Eigen::Vector3d p0, Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3, Eigen::Vector3d p4, Eigen::Vector3d p5, Eigen::Vector3d p6,
+     304           0 :                    Eigen::Vector3d p7)
+     305             :     {
+     306           0 :       points.push_back(p0);
+     307           0 :       points.push_back(p1);
+     308           0 :       points.push_back(p2);
+     309           0 :       points.push_back(p3);
+     310           0 :       points.push_back(p4);
+     311           0 :       points.push_back(p5);
+     312           0 :       points.push_back(p6);
+     313           0 :       points.push_back(p7);
+     314           0 :     }
+     315             : 
+     316           0 :     Cuboid::Cuboid(std::vector<Eigen::Vector3d> points)
+     317             :     {
+     318           0 :       this->points = points;
+     319           0 :     }
+     320             : 
+     321           0 :     Cuboid::Cuboid(Eigen::Vector3d center, Eigen::Vector3d size, Eigen::Quaterniond orientation)
+     322             :     {
+     323           0 :       Eigen::Vector3d p0(size.x() / 2.0, -size.y() / 2.0, -size.z() / 2.0);
+     324           0 :       Eigen::Vector3d p1(size.x() / 2.0, size.y() / 2.0, -size.z() / 2.0);
+     325           0 :       Eigen::Vector3d p2(size.x() / 2.0, size.y() / 2.0, size.z() / 2.0);
+     326           0 :       Eigen::Vector3d p3(size.x() / 2.0, -size.y() / 2.0, size.z() / 2.0);
+     327             : 
+     328           0 :       Eigen::Vector3d p4(-size.x() / 2.0, size.y() / 2.0, -size.z() / 2.0);
+     329           0 :       Eigen::Vector3d p5(-size.x() / 2.0, -size.y() / 2.0, -size.z() / 2.0);
+     330           0 :       Eigen::Vector3d p6(-size.x() / 2.0, -size.y() / 2.0, size.z() / 2.0);
+     331           0 :       Eigen::Vector3d p7(-size.x() / 2.0, size.y() / 2.0, size.z() / 2.0);
+     332             : 
+     333           0 :       p0 = center + orientation * p0;
+     334           0 :       p1 = center + orientation * p1;
+     335           0 :       p2 = center + orientation * p2;
+     336           0 :       p3 = center + orientation * p3;
+     337             : 
+     338           0 :       p4 = center + orientation * p4;
+     339           0 :       p5 = center + orientation * p5;
+     340           0 :       p6 = center + orientation * p6;
+     341           0 :       p7 = center + orientation * p7;
+     342             : 
+     343           0 :       points.push_back(p0);
+     344           0 :       points.push_back(p1);
+     345           0 :       points.push_back(p2);
+     346           0 :       points.push_back(p3);
+     347           0 :       points.push_back(p4);
+     348           0 :       points.push_back(p5);
+     349           0 :       points.push_back(p6);
+     350           0 :       points.push_back(p7);
+     351           0 :     }
+     352             : 
+     353           0 :     Cuboid::~Cuboid()
+     354             :     {
+     355           0 :     }
+     356             :     //}
+     357             : 
+     358             :     /* lookupPoints //{ */
+     359           0 :     std::vector<Eigen::Vector3d> Cuboid::lookupPoints(int face_idx) const
+     360             :     {
+     361           0 :       std::vector<Eigen::Vector3d> lookup;
+     362           0 :       switch (face_idx)
+     363             :       {
+     364           0 :         case Cuboid::FRONT:
+     365           0 :           lookup.push_back(points[0]);
+     366           0 :           lookup.push_back(points[1]);
+     367           0 :           lookup.push_back(points[2]);
+     368           0 :           lookup.push_back(points[3]);
+     369           0 :           break;
+     370           0 :         case Cuboid::BACK:
+     371           0 :           lookup.push_back(points[4]);
+     372           0 :           lookup.push_back(points[5]);
+     373           0 :           lookup.push_back(points[6]);
+     374           0 :           lookup.push_back(points[7]);
+     375           0 :           break;
+     376           0 :         case Cuboid::LEFT:
+     377           0 :           lookup.push_back(points[1]);
+     378           0 :           lookup.push_back(points[4]);
+     379           0 :           lookup.push_back(points[7]);
+     380           0 :           lookup.push_back(points[2]);
+     381           0 :           break;
+     382           0 :         case Cuboid::RIGHT:
+     383           0 :           lookup.push_back(points[5]);
+     384           0 :           lookup.push_back(points[0]);
+     385           0 :           lookup.push_back(points[3]);
+     386           0 :           lookup.push_back(points[6]);
+     387           0 :           break;
+     388           0 :         case Cuboid::BOTTOM:
+     389           0 :           lookup.push_back(points[5]);
+     390           0 :           lookup.push_back(points[4]);
+     391           0 :           lookup.push_back(points[1]);
+     392           0 :           lookup.push_back(points[0]);
+     393           0 :           break;
+     394           0 :         case Cuboid::TOP:
+     395           0 :           lookup.push_back(points[3]);
+     396           0 :           lookup.push_back(points[2]);
+     397           0 :           lookup.push_back(points[7]);
+     398           0 :           lookup.push_back(points[6]);
+     399           0 :           break;
+     400             :       }
+     401           0 :       return lookup;
+     402             :     }
+     403             :     //}
+     404             : 
+     405             :     /* getters //{ */
+     406           0 :     const std::vector<Eigen::Vector3d> Cuboid::vertices() const
+     407             :     {
+     408           0 :       return points;
+     409             :     }
+     410             : 
+     411           0 :     const Rectangle Cuboid::getRectangle(int face_idx) const
+     412             :     {
+     413           0 :       return Rectangle(lookupPoints(face_idx));
+     414             :     }
+     415             : 
+     416           0 :     const Eigen::Vector3d Cuboid::center() const
+     417             :     {
+     418           0 :       Eigen::Vector3d point_sum = points[0];
+     419           0 :       for (int i = 1; i < 8; i++)
+     420             :       {
+     421           0 :         point_sum += points[i];
+     422             :       }
+     423           0 :       return point_sum / 8.0;
+     424             :     }
+     425             :     //}
+     426             : 
+     427             :     /* intersectionRay //{ */
+     428           0 :     const std::vector<Eigen::Vector3d> Cuboid::intersectionRay(Ray r, double epsilon) const
+     429             :     {
+     430           0 :       std::vector<Eigen::Vector3d> ret;
+     431           0 :       for (int i = 0; i < 6; i++)
+     432             :       {
+     433           0 :         Rectangle side = getRectangle(i);
+     434           0 :         auto side_intersect = side.intersectionRay(r, epsilon);
+     435           0 :         if (side_intersect != boost::none)
+     436             :         {
+     437           0 :           ret.push_back(side_intersect.get());
+     438             :         }
+     439             :       }
+     440           0 :       return ret;
+     441             :     }
+     442             :     //}
+     443             : 
+     444             :     //}
+     445             : 
+     446             :     /* Ellipse //{ */
+     447             : 
+     448             :     /* constructors //{ */
+     449           0 :     Ellipse::Ellipse()
+     450             :     {
+     451           0 :     }
+     452             : 
+     453           0 :     Ellipse::~Ellipse()
+     454             :     {
+     455           0 :     }
+     456             : 
+     457           0 :     Ellipse::Ellipse(Eigen::Vector3d center, Eigen::Quaterniond orientation, double a, double b)
+     458             :     {
+     459           0 :       center_point = center;
+     460           0 :       absolute_orientation = orientation;
+     461           0 :       major_semi = a;
+     462           0 :       minor_semi = b;
+     463           0 :     }
+     464             :     //}
+     465             : 
+     466             :     /* getters //{ */
+     467           0 :     double Ellipse::a() const
+     468             :     {
+     469           0 :       return major_semi;
+     470             :     }
+     471             : 
+     472           0 :     double Ellipse::b() const
+     473             :     {
+     474           0 :       return minor_semi;
+     475             :     }
+     476             : 
+     477           0 :     const Eigen::Vector3d Ellipse::center() const
+     478             :     {
+     479           0 :       return center_point;
+     480             :     }
+     481             : 
+     482           0 :     const Eigen::Quaterniond Ellipse::orientation() const
+     483             :     {
+     484           0 :       return absolute_orientation;
+     485             :     }
+     486             : 
+     487             :     //}
+     488             : 
+     489             :     //}
+     490             : 
+     491             :     /* Cylinder //{ */
+     492             : 
+     493             :     /* constructors //{ */
+     494           0 :     Cylinder::Cylinder()
+     495             :     {
+     496           0 :     }
+     497             : 
+     498           0 :     Cylinder::~Cylinder()
+     499             :     {
+     500           0 :     }
+     501             : 
+     502           0 :     Cylinder::Cylinder(Eigen::Vector3d center, double radius, double height, Eigen::Quaterniond orientation)
+     503             :     {
+     504           0 :       this->center_point = center;
+     505           0 :       this->radius = radius;
+     506           0 :       this->height = height;
+     507           0 :       this->absolute_orientation = orientation;
+     508           0 :     }
+     509             :     //}
+     510             : 
+     511             :     /* getters //{ */
+     512           0 :     const Eigen::Vector3d Cylinder::center() const
+     513             :     {
+     514           0 :       return center_point;
+     515             :     }
+     516             : 
+     517           0 :     const Eigen::Quaterniond Cylinder::orientation() const
+     518             :     {
+     519           0 :       return absolute_orientation;
+     520             :     }
+     521             : 
+     522           0 :     double Cylinder::r() const
+     523             :     {
+     524           0 :       return radius;
+     525             :     }
+     526             : 
+     527           0 :     double Cylinder::h() const
+     528             :     {
+     529           0 :       return height;
+     530             :     }
+     531             : 
+     532           0 :     const Ellipse Cylinder::getCap(int index) const
+     533             :     {
+     534           0 :       Ellipse e;
+     535           0 :       Eigen::Vector3d ellipse_center;
+     536           0 :       switch (index)
+     537             :       {
+     538           0 :         case Cylinder::BOTTOM:
+     539           0 :           ellipse_center = center() - orientation() * (Eigen::Vector3d::UnitZ() * (h() / 2.0));
+     540           0 :           e = Ellipse(ellipse_center, orientation(), r(), r());
+     541           0 :           break;
+     542           0 :         case Cylinder::TOP:
+     543           0 :           ellipse_center = center() + orientation() * (Eigen::Vector3d::UnitZ() * (h() / 2.0));
+     544           0 :           e = Ellipse(ellipse_center, orientation(), r(), r());
+     545           0 :           break;
+     546             :       }
+     547           0 :       return e;
+     548             :     }
+     549             : 
+     550             :     //}
+     551             : 
+     552             :     //}
+     553             : 
+     554             :     /* Cone //{ */
+     555             : 
+     556             :     /* constructors //{ */
+     557           0 :     Cone::Cone()
+     558             :     {
+     559           0 :     }
+     560             : 
+     561           0 :     Cone::~Cone()
+     562             :     {
+     563           0 :     }
+     564             : 
+     565           0 :     Cone::Cone(Eigen::Vector3d origin_point, double angle, double height, Eigen::Vector3d absolute_direction)
+     566             :     {
+     567           0 :       this->origin_point = origin_point;
+     568           0 :       this->angle = angle;
+     569           0 :       this->height = height;
+     570           0 :       this->absolute_direction = absolute_direction.normalized();
+     571           0 :     }
+     572             :     //}
+     573             : 
+     574             :     /* getters //{ */
+     575           0 :     const Eigen::Vector3d Cone::origin() const
+     576             :     {
+     577           0 :       return origin_point;
+     578             :     }
+     579             : 
+     580           0 :     const Eigen::Vector3d Cone::direction() const
+     581             :     {
+     582           0 :       return absolute_direction;
+     583             :     }
+     584             : 
+     585           0 :     const Eigen::Vector3d Cone::center() const
+     586             :     {
+     587           0 :       return origin() + (0.5 * h()) * direction();
+     588             :     }
+     589             : 
+     590           0 :     double Cone::theta() const
+     591             :     {
+     592           0 :       return angle;
+     593             :     }
+     594             : 
+     595           0 :     double Cone::h() const
+     596             :     {
+     597           0 :       return height;
+     598             :     }
+     599             : 
+     600           0 :     const Ellipse Cone::getCap() const
+     601             :     {
+     602           0 :       Eigen::Vector3d ellipse_center = origin() + direction() * h();
+     603           0 :       Eigen::Quaterniond ellipse_orientation = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), direction());
+     604           0 :       double cap_radius = std::tan(theta()) * h();
+     605           0 :       Ellipse e(ellipse_center, ellipse_orientation, cap_radius, cap_radius);
+     606           0 :       return e;
+     607             :     }
+     608             : 
+     609           0 :     const std::optional<Eigen::Vector3d> Cone::projectPoint(const Eigen::Vector3d& point) const
+     610             :     {
+     611             : 
+     612           0 :       Eigen::Vector3d point_vec = point - origin();
+     613           0 :       double point_axis_angle = acos((point_vec.dot(direction())) / (point_vec.norm() * direction().norm()));
+     614             : 
+     615             :       /* Eigen::Vector3d axis_projection = this->cone_axis_projector * point_vec + origin(); */
+     616             : 
+     617           0 :       Eigen::Vector3d axis_rot = direction().cross(point_vec);
+     618           0 :       axis_rot.normalize();
+     619             : 
+     620           0 :       Eigen::AngleAxis<double> my_quat(this->angle - point_axis_angle, axis_rot);
+     621             : 
+     622           0 :       Eigen::Vector3d point_on_cone = my_quat * point_vec + origin();
+     623             : 
+     624           0 :       Eigen::Vector3d vec_point_on_cone = point_on_cone - origin();
+     625           0 :       vec_point_on_cone.normalize();
+     626             : 
+     627           0 :       double beta = this->angle - point_axis_angle;
+     628             : 
+     629           0 :       if (point_axis_angle < this->angle)
+     630             :       {
+     631           0 :         return origin() + vec_point_on_cone * cos(beta) * point_vec.norm();
+     632           0 :       } else if ((point_axis_angle >= this->angle) && (point_axis_angle - this->angle) <= M_PI / 2.0)
+     633             :       {  // TODO: is this condition correct?
+     634           0 :         return origin() + vec_point_on_cone * cos(point_axis_angle - this->angle) * point_vec.norm();
+     635             :       } else
+     636             :       {
+     637           0 :         return {};
+     638             :       }
+     639             :     }
+     640             : 
+     641             :     //}
+     642             : 
+     643             :     //}
+     644             : 
+     645             :   }  // namespace geometry
+     646             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/shapes.cpp.gcov.overview.html b/mrs_lib/src/geometry/shapes.cpp.gcov.overview.html new file mode 100644 index 0000000000..5be5367fd4 --- /dev/null +++ b/mrs_lib/src/geometry/shapes.cpp.gcov.overview.html @@ -0,0 +1,182 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/geometry/shapes.cpp.gcov.png b/mrs_lib/src/geometry/shapes.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a648ccdf3602fb84c64b5d246adb511476145fd2 GIT binary patch literal 1596 zcmV-C2E+M@P)t0{{R3FUJ9e0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpQ- z-wPHl5SPm+yO6z&PYOr^P9n+1@kR3M{@Z_g3O@S#`ToIR+h+E%QRNNPB1r)ds^Rpk*;#9^U&pUfTf}2cP8+^N zcKL2HZVH|n(n7|XmFis*GV~H7EzUGkmf6l3lNIgzD17Rs?g{F1xqFzUM8*vixNau% z{;hSzxJ@4GG1@~ak60qxRj`E^nMPMZpBCVOz_C7sQ%en+;lGl14+S3r9N9gX>@Rmd zDl7J?>-CS!UCEe|D9j}gz45${U7)gb2#HDesLDzbB`SP6xbAt&(zbfMALn*Y zq7)b~u&zQ=)(G$UtOFS=PT|Nf%$|&lo3ByPOuj9MtO>bd)>Jpnw&8LdHppNrjZH$R^`ywOrVs^ zt%0oKHC&(U_%V-sgh~XIR*h@XHwII?o_>-CQ^}DL%xi7a0Q=U6O+%VR)JZg(XF7Tw`$vIbl+3uU|>xeqYY5#>rfba%` zNra!8RW*>4h6a)nFOdm>F#Ay+(#gR8luRhMHKZ#5$JxhTuIv2tl`t7^ z(k&ui2whm;)%|wo-f0Bd%X05y}uucO218WNSWBom>|4$9az2@m;#;~T&&des$ zm9;c*P6B5K&Sj@ajslL9#o?DO!*kR7`o!l_V(}EdKHWW%4CGTddC|OW*0pTqz(|}X z+^&{ch@T|C!7OY_ex^ES6NwGUPbjZ==g_f^`LJq|B>EJPf0|}emX z4M6G~vIICgmcn!5yuXeqP@CUF07v*eUq$BR#1&{YdZhEnBo4~OLpJymY=g)wv}k_I z+f7C;NJO0C7Hv#dO@I7UwJw^(y4kS0ik4whUx}AlG)KJ!;XbB0BeaBRhBcQSHMv5D z1pde{)J!M(b^VbO{BaWigp(CXoin?qCA&u)$PcYl{-A|1S#ZN>2RuhSY`e3l>ozby z^Z7Q4VtXX`e|8J|9{QBJh5bfmoXgcOz&6}1jO&9oiF0_tEv$@E0NSa|k|=Zwvxgw! zKrHJP_Ilox?C{0H4+x(Rk?A!u+9fM0@7!6-EhO+m$zW6#^>J&IT1<102^_EZqP*0G z*X^l+voLySg*Wa6_d2%$UcTSJ&vdy8$ zk{muhlULEXznaP9YLk=kSfq_?GF#>W#4DA2*CG*aPc%5Vpqty6%$QtYA$4A{LDH%f zobkoZmr0CWizmcHS$k!2lajU9N{m@}6E9VIn+5LQY2zP1DEB2+bwco{nnC6vNXEOW u8QsJ6Nc;aklm)h=&%Y<@AI1P2+Jt}f$c2VCd;yIB0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/mathHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
math.cpp +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
<unnamed>100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/index-detail-sort-l.html b/mrs_lib/src/math/index-detail-sort-l.html new file mode 100644 index 0000000000..4ad108a63e --- /dev/null +++ b/mrs_lib/src/math/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/mathHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
math.cpp +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
<unnamed>100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/index-detail.html b/mrs_lib/src/math/index-detail.html new file mode 100644 index 0000000000..2617e1a4f4 --- /dev/null +++ b/mrs_lib/src/math/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/mathHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
math.cpp +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
<unnamed>100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/index-sort-f.html b/mrs_lib/src/math/index-sort-f.html new file mode 100644 index 0000000000..0e60f2e147 --- /dev/null +++ b/mrs_lib/src/math/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/mathHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
math.cpp +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/index-sort-l.html b/mrs_lib/src/math/index-sort-l.html new file mode 100644 index 0000000000..3169348012 --- /dev/null +++ b/mrs_lib/src/math/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/mathHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
math.cpp +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/index.html b/mrs_lib/src/math/index.html new file mode 100644 index 0000000000..b8f8ad7f32 --- /dev/null +++ b/mrs_lib/src/math/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/mathHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
math.cpp +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/math.cpp.func-sort-c.html b/mrs_lib/src/math/math.cpp.func-sort-c.html new file mode 100644 index 0000000000..7e8c786a6f --- /dev/null +++ b/mrs_lib/src/math/math.cpp.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math/math.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/math - math.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::probit(double)202
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/math.cpp.func.html b/mrs_lib/src/math/math.cpp.func.html new file mode 100644 index 0000000000..b6945211ee --- /dev/null +++ b/mrs_lib/src/math/math.cpp.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math/math.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/math - math.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::probit(double)202
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/math.cpp.gcov.frameset.html b/mrs_lib/src/math/math.cpp.gcov.frameset.html new file mode 100644 index 0000000000..55135a4804 --- /dev/null +++ b/mrs_lib/src/math/math.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math/math.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/math/math.cpp.gcov.html b/mrs_lib/src/math/math.cpp.gcov.html new file mode 100644 index 0000000000..05e43740a4 --- /dev/null +++ b/mrs_lib/src/math/math.cpp.gcov.html @@ -0,0 +1,149 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math/math.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/math - math.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1717100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/math.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             :   /* probit() function //{ */
+       6         202 :   double probit(const double quantile)
+       7             :   {
+       8             :     // polynomial coefficients of the numerator for rational polynomial approximation in the range (0.08; 0.92)
+       9             :     constexpr double a[4] =
+      10             :     {
+      11             :       2.50662823884,
+      12             :     -18.61500062529,
+      13             :      41.39119773534,
+      14             :     -25.44106049637
+      15             :     };
+      16             : 
+      17             :     // polynomial coefficients of the denominator for the rational polynomial approximation in the range (0.08; 0.92)
+      18             :     constexpr double b[4] =
+      19             :     {
+      20             :       -8.47351093090,
+      21             :       23.08336743743,
+      22             :      -21.06224101826,
+      23             :        3.13082909833
+      24             :     };
+      25             : 
+      26             :     // polynomial coefficients of the logarithmical approximation in the range (0; 0.08) U (0.92; 1)
+      27             :     constexpr double c[9] =
+      28             :     {
+      29             :       0.3374754822726147,
+      30             :       0.9761690190917186,
+      31             :       0.1607979714918209,
+      32             :       0.0276438810333863,
+      33             :       0.0038405729373609,
+      34             :       0.0003951896511919,
+      35             :       0.0000321767881768,
+      36             :       0.0000002888167364,
+      37             :       0.0000003960315187
+      38             :     };
+      39             : 
+      40             :     // correctly handle special values
+      41         202 :     if (quantile == 1.0)
+      42           1 :       return std::numeric_limits<double>::infinity();
+      43         201 :     if (quantile == 0.0)
+      44           1 :       return -std::numeric_limits<double>::infinity();
+      45         200 :     if (quantile < 0.0 || quantile > 1.0)
+      46           2 :       return std::numeric_limits<double>::quiet_NaN();
+      47             : 
+      48         198 :     const double y = quantile - 0.5;
+      49         198 :     if (std::abs(y) < 0.42)
+      50             :     {
+      51         168 :       const double r = y*y;
+      52         168 :       const double num = y*((( a[3]*r + a[2] )*r + a[1])*r + a[0]);
+      53         168 :       const double denom = (((( b[3]*r + b[2] )*r + b[1])*r + b[0])*r + 1);
+      54         168 :       return num/denom;
+      55             :     }
+      56             :     else
+      57             :     {
+      58          30 :       const double v = y > 0 ? 1.0 - quantile : quantile;
+      59          30 :       const double r = std::log(-std::log(v));
+      60          30 :       const double x = c[0] + r*( c[1] + r*( c[2] + r*( c[3] + r*( c[4] + r*( c[5] + r*( c[6] + r*( c[7] + r*c[8] )))))));
+      61          30 :       return y < 0 ? -x : x;
+      62             :     }
+      63             :   }
+      64             :   //}
+      65             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/math/math.cpp.gcov.overview.html b/mrs_lib/src/math/math.cpp.gcov.overview.html new file mode 100644 index 0000000000..8ea3ed0ce0 --- /dev/null +++ b/mrs_lib/src/math/math.cpp.gcov.overview.html @@ -0,0 +1,37 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/math/math.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/math/math.cpp.gcov.png b/mrs_lib/src/math/math.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..11021a1f7a3fe1b16c9ed60780115ac61956a437 GIT binary patch literal 388 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1?!VDz0Y_(eeq$C1-LR|m<|Gx?dmcNgUef6J# zVHHpuOr7)IycEdhEbxddW?X?_wfUqO7#M{-T^vI^I^TxHN;NC+EH*!&eSr7D zzjEnZM%SO5_wU^Pv> + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
median_filter.cpp +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
<unnamed>89.1 %82 / 9294.1 %16 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/index-detail-sort-l.html b/mrs_lib/src/median_filter/index-detail-sort-l.html new file mode 100644 index 0000000000..73f9d256e6 --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
median_filter.cpp +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
<unnamed>89.1 %82 / 9294.1 %16 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/index-detail.html b/mrs_lib/src/median_filter/index-detail.html new file mode 100644 index 0000000000..0ce1f117fb --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
median_filter.cpp +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
<unnamed>89.1 %82 / 9294.1 %16 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/index-sort-f.html b/mrs_lib/src/median_filter/index-sort-f.html new file mode 100644 index 0000000000..b018929c13 --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
median_filter.cpp +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/index-sort-l.html b/mrs_lib/src/median_filter/index-sort-l.html new file mode 100644 index 0000000000..76d9f33080 --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
median_filter.cpp +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/index.html b/mrs_lib/src/median_filter/index.html new file mode 100644 index 0000000000..1e9ce0b881 --- /dev/null +++ b/mrs_lib/src/median_filter/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
median_filter.cpp +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html b/mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html new file mode 100644 index 0000000000..118e3bf24c --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::setMaxValue(double)2
mrs_lib::MedianFilter::setMinValue(double)2
mrs_lib::MedianFilter::setBufferLength(unsigned long)2
mrs_lib::MedianFilter::setMaxDifference(double)2
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)78
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)79
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)81
mrs_lib::MedianFilter::check(double)132412
mrs_lib::MedianFilter::median() const132422
mrs_lib::MedianFilter::full() const140026
mrs_lib::MedianFilter::add(double)140053
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.func.html b/mrs_lib/src/median_filter/median_filter.cpp.func.html new file mode 100644 index 0000000000..ad03fa8692 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::MedianFilter::setMaxValue(double)2
mrs_lib::MedianFilter::setMinValue(double)2
mrs_lib::MedianFilter::setBufferLength(unsigned long)2
mrs_lib::MedianFilter::setMaxDifference(double)2
mrs_lib::MedianFilter::add(double)140053
mrs_lib::MedianFilter::check(double)132412
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)78
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)81
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)79
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::full() const140026
mrs_lib::MedianFilter::median() const132422
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.gcov.frameset.html b/mrs_lib/src/median_filter/median_filter.cpp.gcov.frameset.html new file mode 100644 index 0000000000..13e545bd2d --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.gcov.html b/mrs_lib/src/median_filter/median_filter.cpp.gcov.html new file mode 100644 index 0000000000..8743c05da5 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.html @@ -0,0 +1,286 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-25 05:42:57Functions:161794.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/median_filter.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             :   /* constructor overloads //{ */
+       6             : 
+       7          81 :   MedianFilter::MedianFilter(const size_t buffer_length, const double min_value, const double max_value, const double max_diff)
+       8             :     : m_median(std::nullopt),
+       9             :       m_min_valid(min_value),
+      10             :       m_max_valid(max_value),
+      11          81 :       m_max_diff(max_diff)
+      12             :   {
+      13          81 :     m_buffer.set_capacity(buffer_length);
+      14          81 :     m_buffer_sorted.reserve(buffer_length);
+      15          81 :   }
+      16             : 
+      17           1 :   MedianFilter::MedianFilter()
+      18             :     : m_median(std::nullopt),
+      19             :       m_min_valid(0.0),
+      20             :       m_max_valid(0.0),
+      21           1 :       m_max_diff(0.0)
+      22             :   {
+      23           1 :     m_buffer.set_capacity(0);
+      24           1 :   }
+      25             : 
+      26           1 :   MedianFilter::MedianFilter(const MedianFilter& other)
+      27             :   {
+      28           1 :     *this = other;
+      29           1 :   }
+      30             : 
+      31          78 :   MedianFilter::MedianFilter(MedianFilter&& other)
+      32             :   {
+      33          78 :     *this = other;
+      34          78 :   }
+      35             : 
+      36             :   //}
+      37             : 
+      38             :   /* operator=() method and overloads //{ */
+      39          79 :   MedianFilter& MedianFilter::operator=(const MedianFilter& other)
+      40             :   {
+      41          79 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      42             :   
+      43          79 :     m_buffer = other.m_buffer;
+      44          79 :     m_buffer_sorted = other.m_buffer_sorted;
+      45          79 :     m_median = other.m_median;
+      46             :   
+      47             :     // parameters specified by the user
+      48          79 :     m_min_valid = other.m_min_valid;
+      49          79 :     m_max_valid = other.m_max_valid;
+      50          79 :     m_max_diff = other.m_max_diff;
+      51             :   
+      52         158 :     return *this;
+      53             :   }
+      54             : 
+      55           0 :   MedianFilter& MedianFilter::operator=(MedianFilter&& other)
+      56             :   {
+      57           0 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      58             : 
+      59           0 :     m_buffer = std::move(other.m_buffer);
+      60           0 :     m_buffer_sorted = std::move(other.m_buffer_sorted);
+      61           0 :     m_median = std::move(other.m_median);
+      62             : 
+      63             :     // parameters specified by the user
+      64           0 :     m_min_valid = other.m_min_valid;
+      65           0 :     m_max_valid = other.m_max_valid;
+      66           0 :     m_max_diff = other.m_max_diff;
+      67             : 
+      68           0 :     return *this;
+      69             :   }
+      70             :   //}
+      71             : 
+      72             :   /* add() method //{ */
+      73      140053 :   void MedianFilter::add(const double value)
+      74             :   {
+      75      280058 :     std::scoped_lock lck(m_mtx);
+      76             :     // add the value to the buffer
+      77      140044 :     m_buffer.push_back(value);
+      78             :     // reset the cached median value
+      79      140033 :     m_median = std::nullopt;
+      80      140025 :   }
+      81             :   //}
+      82             : 
+      83             :   /* check() method //{ */
+      84      132412 :   bool MedianFilter::check(const double value)
+      85             :   {
+      86      132412 :     std::scoped_lock lck(m_mtx);
+      87             :     // check if all constraints are met
+      88      132411 :     const double diff = m_buffer.empty() ? 0.0 : std::abs(median() - value);
+      89      264831 :     return value > m_min_valid && value < m_max_valid && diff < m_max_diff;
+      90             :   }
+      91             :   //}
+      92             : 
+      93             :   /* addCheck() method //{ */
+      94          26 :   bool MedianFilter::addCheck(const double value)
+      95             :   {
+      96          52 :     std::scoped_lock lck(m_mtx);
+      97          26 :     add(value);
+      98          52 :     return check(value);
+      99             :   }
+     100             :   //}
+     101             : 
+     102             :   /* clear() method //{ */
+     103           2 :   void MedianFilter::clear()
+     104             :   {
+     105           4 :     std::scoped_lock lck(m_mtx);
+     106           2 :     m_median = std::nullopt;
+     107           2 :     m_buffer.clear();
+     108           2 :   }
+     109             :   //}
+     110             : 
+     111             :   /* full() method //{ */
+     112      140026 :   bool MedianFilter::full() const
+     113             :   {
+     114      280056 :     std::scoped_lock lck(m_mtx);
+     115      280064 :     return m_buffer.full();
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* median() method //{ */
+     120      132422 :   double MedianFilter::median() const
+     121             :   {
+     122      264855 :     std::scoped_lock lck(m_mtx);
+     123             :     // if the value was already calculated, just return it
+     124      132415 :     if (m_median.has_value())
+     125          17 :       return m_median.value();
+     126             :   
+     127             :     // check if there are even any numbers to calculate the median from
+     128      132399 :     if (m_buffer.empty())
+     129             :     {
+     130           4 :       m_median = std::numeric_limits<double>::quiet_NaN();
+     131           4 :       return m_median.value();
+     132             :     }
+     133             :   
+     134             :     // remove any elements from buffer_sorted
+     135      132398 :     m_buffer_sorted.clear();
+     136             :     // copy all elements from the input buffer to buffer_sorted
+     137      132400 :     m_buffer_sorted.insert(std::end(m_buffer_sorted), std::begin(m_buffer), std::end(m_buffer));
+     138             :     // check for the special case of the median when there is an even number of numbers in the set
+     139      132421 :     const bool even_set = m_buffer_sorted.size() % 2 == 0;
+     140             :   
+     141             :     // if it's an even set, we'll need one more element sorted than for an odd set of numbers
+     142      132389 :     const size_t median_pos = even_set ? std::ceil(m_buffer_sorted.size()/2.0) : std::floor(m_buffer_sorted.size()/2.0);
+     143             :     // actually sort the elements in buffer_sorted up to the n-th element
+     144      132388 :     std::nth_element(std::begin(m_buffer_sorted), std::begin(m_buffer_sorted)+median_pos, std::end(m_buffer_sorted));
+     145             :   
+     146             :     // special case for a median of an even set of numbers
+     147      132424 :     if (even_set)
+     148      132408 :       m_median = (m_buffer_sorted.at(median_pos) + m_buffer_sorted.at(median_pos-1))/2.0;
+     149             :     // the "normal" case with an odd set
+     150             :     else
+     151          16 :       m_median = m_buffer_sorted.at(median_pos);
+     152             :     // return the now-cached value
+     153      132419 :     return m_median.value();
+     154             :   }
+     155             :   //}
+     156             : 
+     157             :   /* initialized() method //{ */
+     158           1 :   bool MedianFilter::initialized() const
+     159             :   {
+     160           1 :     std::scoped_lock lck(m_mtx);
+     161           2 :     return m_buffer.size() > 0;
+     162             :   }
+     163             :   //}
+     164             : 
+     165             :   /* setBufferLength() method //{ */
+     166           2 :   void MedianFilter::setBufferLength(const size_t buffer_length)
+     167             :   {
+     168           4 :     std::scoped_lock lck(m_mtx);
+     169             :     // the median may change if the some values are discarded
+     170           2 :     if (buffer_length < m_buffer.size())
+     171           0 :       m_median = std::nullopt;
+     172             :   
+     173           2 :     m_buffer.set_capacity(buffer_length);
+     174           2 :     m_buffer_sorted.reserve(buffer_length);
+     175           2 :   }
+     176             :   //}
+     177             : 
+     178             :   /* setMinValue() method //{ */
+     179           2 :   void MedianFilter::setMinValue(const double min_value)
+     180             :   {
+     181           2 :     std::scoped_lock lck(m_mtx);
+     182           2 :     m_min_valid = min_value;
+     183           2 :   }
+     184             :   //}
+     185             : 
+     186             :   /* setMaxValue() method //{ */
+     187           2 :   void MedianFilter::setMaxValue(const double max_value)
+     188             :   {
+     189           2 :     std::scoped_lock lck(m_mtx);
+     190           2 :     m_max_valid = max_value;
+     191           2 :   }
+     192             :   //}
+     193             : 
+     194             :   /* setMaxDifference() method //{ */
+     195           2 :   void MedianFilter::setMaxDifference(const double max_diff)
+     196             :   {
+     197           2 :     std::scoped_lock lck(m_mtx);
+     198           2 :     m_max_diff = max_diff;
+     199           2 :   }
+     200             :   //}
+     201             : 
+     202             : } // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html b/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html new file mode 100644 index 0000000000..f9295bd144 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html @@ -0,0 +1,71 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.gcov.png b/mrs_lib/src/median_filter/median_filter.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b9ea102b4e20dc14f5089f33c629964d0722e495 GIT binary patch literal 872 zcmV-u1DE`XP)?#D47~zV4R{20qwmlQQ?*Cb&s)6T zPH}7sC3BcfAv=e&rjVxZbZhD~ zU^@f$!VY5T54!|BMaABIVolQs69${1Vn3^$QQcEvJ~Nb+dFXRy#xW(a*j)(UK=|-d zcn6q4!X+ieTYTFoZcgp#=XXZ9;UUc;T=%K`eBug-jIp$soK(Z1fMXS>VvE~It{8>b za-G0wD$2mPenz?TQJ614j>{nB zm!`rqNz|oy=e2p)lxD^?n*atsLRfUiz|xc5aO}fJdk9+LDj)#qB|0sI{;`?K@ca#G z*d1;}%Ec(V&*xlMV#H)Y-q^-I+ZEo@0nED&>vl9n|HuHJy}R0#$18pTk3A*}9vuO+ z3g_~uL+av28#38siXfrtRV^Or^-SxYLU{e~$?s<-9h + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.cpp +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
<unnamed>80.0 %44 / 55100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/index-detail-sort-l.html b/mrs_lib/src/param_loader/index-detail-sort-l.html new file mode 100644 index 0000000000..229f90cf17 --- /dev/null +++ b/mrs_lib/src/param_loader/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.cpp +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
<unnamed>80.0 %44 / 55100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/index-detail.html b/mrs_lib/src/param_loader/index-detail.html new file mode 100644 index 0000000000..ed8735d6bc --- /dev/null +++ b/mrs_lib/src/param_loader/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.cpp +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
<unnamed>80.0 %44 / 55100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/index-sort-f.html b/mrs_lib/src/param_loader/index-sort-f.html new file mode 100644 index 0000000000..1a15e5a749 --- /dev/null +++ b/mrs_lib/src/param_loader/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.cpp +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/index-sort-l.html b/mrs_lib/src/param_loader/index-sort-l.html new file mode 100644 index 0000000000..684845e427 --- /dev/null +++ b/mrs_lib/src/param_loader/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.cpp +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/index.html b/mrs_lib/src/param_loader/index.html new file mode 100644 index 0000000000..518954bf14 --- /dev/null +++ b/mrs_lib/src/param_loader/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.cpp +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html b/mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html new file mode 100644 index 0000000000..0efd260eb5 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loader - param_provider.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamProvider::getParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&) const4
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)4102
mrs_lib::ParamProvider::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13450
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const89798
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.func.html b/mrs_lib/src/param_loader/param_provider.cpp.func.html new file mode 100644 index 0000000000..e276fd53e3 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loader - param_provider.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamProvider::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13450
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)4102
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const89798
mrs_lib::ParamProvider::getParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&) const4
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.gcov.frameset.html b/mrs_lib/src/param_loader/param_provider.cpp.gcov.frameset.html new file mode 100644 index 0000000000..e288051dac --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.gcov.html b/mrs_lib/src/param_loader/param_provider.cpp.gcov.html new file mode 100644 index 0000000000..e225616e69 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.gcov.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loader - param_provider.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/param_provider.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             :   // Explicit instantiation of the tepmplated functions to precompile them into mrs_lib and speed up compilation of user program.
+       6             :   // Instantiating these functions should be sufficient to invoke precompilation of all templated ParamLoader functions.
+       7             : 
+       8             :   template bool ParamProvider::getParam<bool>(const std::string& name, bool& out_value) const;
+       9             :   template bool ParamProvider::getParam<int>(const std::string& name, int& out_value) const;
+      10             :   template bool ParamProvider::getParam<double>(const std::string& name, double& out_value) const;
+      11             :   template bool ParamProvider::getParam<std::string>(const std::string& name, std::string& out_value) const;
+      12             : 
+      13        4102 :   ParamProvider::ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam)
+      14        4102 :   : m_nh(nh), m_node_name(std::move(node_name)), m_use_rosparam(use_rosparam)
+      15             :   {
+      16        4102 :   }
+      17             : 
+      18       13450 :   bool ParamProvider::addYamlFile(const std::string& filepath)
+      19             :   {
+      20             :     try
+      21             :     {
+      22       26900 :       const auto loaded_yaml = YAML::LoadFile(filepath);
+      23       13450 :       YAML::Node root;
+      24       13450 :       root["root"] = loaded_yaml;
+      25       13450 :       m_yamls.emplace_back(root);
+      26       13450 :       return true;
+      27             :     }
+      28           0 :     catch (const YAML::ParserException& e)
+      29             :     {
+      30           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: Failed to parse file \"" << filepath << "\"! Parameters will not be loaded: " << e.what());
+      31           0 :       return false;
+      32             :     }
+      33           0 :     catch (const YAML::BadFile& e)
+      34             :     {
+      35           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: File \"" << filepath << "\" does not exist! Parameters will not be loaded: " << e.what());
+      36           0 :       return false;
+      37             :     }
+      38           0 :     catch (const YAML::Exception& e)
+      39             :     {
+      40           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: YAML-CPP threw an exception! Parameters will not be loaded: " << e.what());
+      41           0 :       return false;
+      42             :     }
+      43             :     return false;
+      44             :   }
+      45             : 
+      46           4 :   bool ParamProvider::getParam(const std::string& param_name, XmlRpc::XmlRpcValue& value_out) const
+      47             :   {
+      48           4 :     if (m_use_rosparam && m_nh.getParam(param_name, value_out))
+      49           2 :       return true;
+      50             : 
+      51             :     try
+      52             :     {
+      53           4 :       const auto found_node = findYamlNode(param_name);
+      54           2 :       if (found_node.has_value())
+      55           1 :         ROS_WARN_STREAM("[" << m_node_name << "]: Parameter \"" << param_name << "\" of desired type XmlRpc::XmlRpcValue is only available as a static parameter, which doesn't support loading of this type.");
+      56             :     }
+      57           0 :     catch (const YAML::Exception& e)
+      58             :     {
+      59           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: YAML-CPP threw an unknown exception: " << e.what());
+      60             :     }
+      61           2 :     return false;
+      62             :   }
+      63             : 
+      64       89798 :   std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
+      65             :   {
+      66      464592 :     for (const auto& yaml : m_yamls)
+      67             :     {
+      68             :       // Try to load the parameter sequentially as a map.
+      69      448619 :       auto cur_node_it = std::cbegin(yaml);
+      70             :       // The root should always be a pam
+      71      448614 :       if (!cur_node_it->second.IsMap())
+      72       14053 :         continue;
+      73             : 
+      74      434565 :       bool loaded = true;
+      75             :       {
+      76      434565 :         constexpr char delimiter = '/';
+      77      434565 :         auto substr_start = std::cbegin(param_name);
+      78      434567 :         auto substr_end = substr_start;
+      79      592792 :         do
+      80             :         {
+      81     1027359 :           substr_end = std::find(substr_start, std::cend(param_name), delimiter);
+      82             :           // why can't substr or string_view take iterators? :'(
+      83     1027353 :           const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
+      84     1027352 :           const auto count = std::distance(substr_start, substr_end);
+      85     1027346 :           const std::string param_substr = param_name.substr(start_pos, count);
+      86     1027349 :           substr_start = substr_end+1;
+      87             : 
+      88     1027343 :           bool found = false;
+      89     4206421 :           for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
+      90             :           {
+      91     2151723 :             if (node_it->first.as<std::string>() == param_substr)
+      92             :             {
+      93      666619 :               cur_node_it = node_it;
+      94      666618 :               found = true;
+      95      666618 :               break;
+      96             :             }
+      97             :           }
+      98             : 
+      99     1027363 :           if (!found)
+     100             :           {
+     101      360743 :             loaded = false;
+     102      360743 :             break;
+     103             :           }
+     104             :         }
+     105      666615 :         while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
+     106             :       }
+     107             : 
+     108      434568 :       if (loaded)
+     109             :       {
+     110      147651 :         return cur_node_it->second;
+     111             :       }
+     112             :     }
+     113             : 
+     114       15969 :     return std::nullopt;
+     115             :   }
+     116             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html b/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html new file mode 100644 index 0000000000..af67093666 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html @@ -0,0 +1,49 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.gcov.png b/mrs_lib/src/param_loader/param_provider.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..774efb61cfe2d289995153e385a5e1b80dae2dc4 GIT binary patch literal 659 zcmV;E0&M+>P)UH@$AI|+MB1M3`qHF|E!oDf?bgK3?6rMp-U;Fe zkK`G?h@ZU?F^0bj--Qbpr3exw7(yf$`=T0=USw(Ffy90f6cO$@4uGZ05wP@%YmrB7 zimuxJg{!ko>U`6K(@W9(#RgL{-7JRvoEN({pfV zciJ*A6>A8b1@=HJk=uD1a~WgaCfQ}q)-3SqJnjO|?CX7z%eRZ*F82TOUkKRnl`VCWx@JzdJFITnc=4JpY|qGF~J04QPdN+K(K_HKko^Dt`huEBt+TKQp$D@?gXKJRFSB5@EeY@o&a6asd48e1TBei zIUIR)Kq!%!BPGd-9*Ln*S2V>vFyaOtQI3e3kjJXjM=`GB8P@|_d_6*fA8!F3{ne_w%LLQOOaCSoAxMXq@>8hddvA2?N tAl(lpE>rQT$L}9 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profilerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
<unnamed>37.6 %38 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/index-detail-sort-l.html b/mrs_lib/src/profiler/index-detail-sort-l.html new file mode 100644 index 0000000000..1bd5ca1cef --- /dev/null +++ b/mrs_lib/src/profiler/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profilerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
<unnamed>37.6 %38 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/index-detail.html b/mrs_lib/src/profiler/index-detail.html new file mode 100644 index 0000000000..fd9c3d6857 --- /dev/null +++ b/mrs_lib/src/profiler/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profilerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
<unnamed>37.6 %38 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/index-sort-f.html b/mrs_lib/src/profiler/index-sort-f.html new file mode 100644 index 0000000000..d942633991 --- /dev/null +++ b/mrs_lib/src/profiler/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profilerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/index-sort-l.html b/mrs_lib/src/profiler/index-sort-l.html new file mode 100644 index 0000000000..1784dbc1d2 --- /dev/null +++ b/mrs_lib/src/profiler/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profilerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/index.html b/mrs_lib/src/profiler/index.html new file mode 100644 index 0000000000..bab4fca89a --- /dev/null +++ b/mrs_lib/src/profiler/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profilerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html new file mode 100644 index 0000000000..6dffdb3815 --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Profiler::Profiler(mrs_lib::Profiler const&)0
mrs_lib::Profiler::Profiler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)1317
mrs_lib::Profiler::Profiler()1317
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1317
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)678980
mrs_lib::Routine::Routine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, std::shared_ptr<ros::Publisher>, std::shared_ptr<std::mutex>, bool, ros::TimerEvent)679087
mrs_lib::Routine::Routine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::shared_ptr<ros::Publisher>, std::shared_ptr<std::mutex>, bool)1897892
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)1897919
mrs_lib::Routine::end()2576810
mrs_lib::Routine::~Routine()2576904
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.func.html b/mrs_lib/src/profiler/profiler.cpp.func.html new file mode 100644 index 0000000000..ce696bfbcc --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Routine::end()2576810
mrs_lib::Routine::Routine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::shared_ptr<ros::Publisher>, std::shared_ptr<std::mutex>, bool)1897892
mrs_lib::Routine::Routine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, std::shared_ptr<ros::Publisher>, std::shared_ptr<std::mutex>, bool, ros::TimerEvent)679087
mrs_lib::Routine::~Routine()2576904
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)1897919
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)678980
mrs_lib::Profiler::Profiler(mrs_lib::Profiler const&)0
mrs_lib::Profiler::Profiler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)1317
mrs_lib::Profiler::Profiler()1317
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1317
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.frameset.html b/mrs_lib/src/profiler/profiler.cpp.gcov.frameset.html new file mode 100644 index 0000000000..c91c4fd036 --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.html b/mrs_lib/src/profiler/profiler.cpp.gcov.html new file mode 100644 index 0000000000..c2a5abe1ae --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.gcov.html @@ -0,0 +1,301 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-25 05:42:57Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/profiler.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6             : // | ------------------------ Profiler ------------------------ |
+       7             : 
+       8             : /* Profiler constructor //{ */
+       9             : 
+      10        1317 : Profiler::Profiler() {
+      11        1317 : }
+      12             : 
+      13        1317 : Profiler::Profiler(ros::NodeHandle& nh, std::string _node_name_, bool profiler_enabled) {
+      14             : 
+      15        1317 :   this->nh_                = std::make_shared<ros::NodeHandle>(nh);
+      16        1317 :   this->_node_name_        = _node_name_;
+      17        1317 :   this->_profiler_enabled_ = profiler_enabled;
+      18             : 
+      19        1317 :   if (profiler_enabled) {
+      20           0 :     mutex_publisher_ = std::make_unique<std::mutex>();
+      21           0 :     publisher_       = std::make_unique<ros::Publisher>(this->nh_->advertise<mrs_msgs::ProfilerUpdate>("profiler", 100, false));
+      22             :   }
+      23             : 
+      24        1317 :   ROS_INFO("[%s]: profiler initialized", _node_name_.c_str());
+      25             : 
+      26        1317 :   this->is_initialized_ = true;
+      27        1317 : }
+      28             : 
+      29           0 : Profiler::Profiler(const Profiler& other) {
+      30             : 
+      31           0 :   this->is_initialized_    = other.is_initialized_;
+      32           0 :   this->nh_                = other.nh_;
+      33           0 :   this->_node_name_        = other._node_name_;
+      34           0 :   this->_profiler_enabled_ = other._profiler_enabled_;
+      35             : 
+      36           0 :   if (this->_profiler_enabled_ && this->is_initialized_) {
+      37           0 :     mutex_publisher_ = std::make_unique<std::mutex>();
+      38           0 :     publisher_       = std::make_unique<ros::Publisher>(this->nh_->advertise<mrs_msgs::ProfilerUpdate>("profiler", 100, false));
+      39             :   }
+      40           0 : }
+      41             : 
+      42        1317 : Profiler& Profiler::operator=(const Profiler& other) {
+      43             : 
+      44        1317 :   if (this == &other) {
+      45           0 :     return *this;
+      46             :   }
+      47             : 
+      48        1317 :   this->is_initialized_    = other.is_initialized_;
+      49        1317 :   this->nh_                = other.nh_;
+      50        1317 :   this->_node_name_        = other._node_name_;
+      51        1317 :   this->_profiler_enabled_ = other._profiler_enabled_;
+      52             : 
+      53        1317 :   if (this->_profiler_enabled_ && this->is_initialized_) {
+      54           0 :     mutex_publisher_ = std::make_unique<std::mutex>();
+      55           0 :     publisher_       = std::make_unique<ros::Publisher>(this->nh_->advertise<mrs_msgs::ProfilerUpdate>("profiler", 100, false));
+      56             :   }
+      57             : 
+      58        1317 :   return *this;
+      59             : }
+      60             : 
+      61             : //}
+      62             : 
+      63             : /* Profiler::registerRoutine() for periodic //{ */
+      64             : 
+      65      678980 : Routine Profiler::createRoutine(std::string name, double expected_rate, double threshold, ros::TimerEvent event) {
+      66             : 
+      67      678980 :   return Routine(name, this->_node_name_, expected_rate, threshold, publisher_, mutex_publisher_, _profiler_enabled_, event);
+      68             : }
+      69             : 
+      70             : //}
+      71             : 
+      72             : /* Profiler::registerRoutine() normal //{ */
+      73             : 
+      74     1897919 : Routine Profiler::createRoutine(std::string name) {
+      75             : 
+      76     1897919 :   return Routine(name, this->_node_name_, publisher_, mutex_publisher_, _profiler_enabled_);
+      77             : }
+      78             : 
+      79             : //}
+      80             : 
+      81             : // | ------------------------- Routine ------------------------ |
+      82             : 
+      83             : /* Routine constructor for periodic //{ */
+      84             : 
+      85      679087 : Routine::Routine(std::string name, std::string node_name, double expected_rate, double threshold, std::shared_ptr<ros::Publisher> publisher,
+      86      679087 :                  std::shared_ptr<std::mutex> mutex_publisher, bool profiler_enabled, ros::TimerEvent event) {
+      87             : 
+      88      678246 :   if (!profiler_enabled) {
+      89      678270 :     return;
+      90             :   }
+      91             : 
+      92           0 :   _threshold_ = threshold;
+      93             : 
+      94           0 :   this->publisher_       = publisher;
+      95           0 :   this->mutex_publisher_ = mutex_publisher;
+      96             : 
+      97           0 :   this->_routine_name_  = name;
+      98           0 :   msg_out_.routine_name = name;
+      99             : 
+     100           0 :   this->_node_name_  = node_name;
+     101           0 :   msg_out_.node_name = node_name;
+     102             : 
+     103           0 :   this->_profiler_enabled_ = profiler_enabled;
+     104             : 
+     105           0 :   msg_out_.is_periodic   = true;
+     106           0 :   msg_out_.expected_rate = expected_rate;
+     107             : 
+     108           0 :   msg_out_.expected_start = event.current_expected.toSec();
+     109           0 :   msg_out_.real_start     = event.current_real.toSec();
+     110             : 
+     111           0 :   msg_out_.stamp     = ros::Time::now();
+     112           0 :   msg_out_.duration  = 0;
+     113           0 :   msg_out_.iteration = this->iteration_++;
+     114           0 :   msg_out_.event     = mrs_msgs::ProfilerUpdate::START;
+     115             : 
+     116           0 :   execution_start_ = ros::Time::now();
+     117             : 
+     118           0 :   double dt = msg_out_.real_start - msg_out_.expected_start;
+     119             : 
+     120           0 :   if (dt > _threshold_) {
+     121           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: routine '%s' was lauched late by %.3f s!", _node_name_.c_str(), _routine_name_.c_str(), dt);
+     122             :   }
+     123             : 
+     124             :   {
+     125           0 :     std::scoped_lock lock(*mutex_publisher_);
+     126             : 
+     127             :     try {
+     128           0 :       publisher_->publish(mrs_msgs::ProfilerUpdateConstPtr(new mrs_msgs::ProfilerUpdate(msg_out_)));
+     129             :     }
+     130           0 :     catch (...) {
+     131           0 :       ROS_ERROR("Exception caught during publishing topic %s.", publisher_->getTopic().c_str());
+     132             :     }
+     133             :   }
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* Routine constructor for normal //{ */
+     139             : 
+     140     1897892 : Routine::Routine(std::string name, std::string node_name, std::shared_ptr<ros::Publisher> publisher, std::shared_ptr<std::mutex> mutex_publisher,
+     141     1897892 :                  bool profiler_enabled) {
+     142             : 
+     143     1897020 :   if (!profiler_enabled) {
+     144     1897027 :     return;
+     145             :   }
+     146             : 
+     147           9 :   this->publisher_       = publisher;
+     148           0 :   this->mutex_publisher_ = mutex_publisher;
+     149             : 
+     150           0 :   this->_routine_name_  = name;
+     151           0 :   msg_out_.routine_name = name;
+     152             : 
+     153           0 :   this->_node_name_  = node_name;
+     154           0 :   msg_out_.node_name = node_name;
+     155             : 
+     156           0 :   this->_profiler_enabled_ = profiler_enabled;
+     157             : 
+     158           0 :   msg_out_.is_periodic   = false;
+     159           0 :   msg_out_.expected_rate = 0;
+     160             : 
+     161           0 :   msg_out_.stamp      = ros::Time::now();
+     162           0 :   msg_out_.duration   = 0;
+     163           0 :   msg_out_.iteration  = this->iteration_++;
+     164           0 :   msg_out_.event      = mrs_msgs::ProfilerUpdate::START;
+     165           0 :   msg_out_.real_start = msg_out_.stamp.toSec();
+     166             : 
+     167           0 :   execution_start_ = ros::Time::now();
+     168             : 
+     169             :   {
+     170           0 :     std::scoped_lock lock(*mutex_publisher_);
+     171             : 
+     172             :     try {
+     173           0 :       publisher_->publish(mrs_msgs::ProfilerUpdateConstPtr(new mrs_msgs::ProfilerUpdate(msg_out_)));
+     174             :     }
+     175           0 :     catch (...) {
+     176           0 :       ROS_ERROR("Exception caught during publishing topic %s.", publisher_->getTopic().c_str());
+     177             :     }
+     178             :   }
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* end() //{ */
+     184             : 
+     185     2576810 : void Routine::end(void) {
+     186             : 
+     187     2576810 :   if (!_profiler_enabled_) {
+     188     2576467 :     return;
+     189             :   }
+     190             : 
+     191         343 :   ros::Time execution_end = ros::Time::now();
+     192             : 
+     193           0 :   msg_out_.stamp    = ros::Time::now();
+     194           0 :   msg_out_.duration = (execution_end - execution_start_).toSec();
+     195             : 
+     196           0 :   msg_out_.event = mrs_msgs::ProfilerUpdate::END;
+     197             : 
+     198             :   {
+     199           0 :     std::scoped_lock lock(*mutex_publisher_);
+     200             : 
+     201             :     try {
+     202           0 :       publisher_->publish(mrs_msgs::ProfilerUpdateConstPtr(new mrs_msgs::ProfilerUpdate(msg_out_)));
+     203             :     }
+     204           0 :     catch (...) {
+     205           0 :       ROS_ERROR("Exception caught during publishing topic %s.", publisher_->getTopic().c_str());
+     206             :     }
+     207             :   }
+     208             : }
+     209             : 
+     210             : //}
+     211             : 
+     212     2576643 : Routine::~Routine() {
+     213             : 
+     214     2576904 :   this->end();
+     215     2575204 : }
+     216             : 
+     217             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html b/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html new file mode 100644 index 0000000000..f66727ff8b --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html @@ -0,0 +1,75 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.png b/mrs_lib/src/profiler/profiler.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..9724b44a6103493cc24901baf8b7a598ae3b23c7 GIT binary patch literal 829 zcmV-D1H$}?P)jX4>?fp(O*C8=7vv$G0st@QIgJxr;(ykZ$z$lG3!tG4g=aZ^cF{d z^}aVvg#$Pj6yd${+5L}WQr79RxG5%sjugjnif}L}vb9PU9VvIAj9upw z=yEW`igkF0TIBsQuHj&-iL2DBh#KJ$n(C&&%Sr&4%-Q|F1Asl42_%@G-Pax#F_2~> z<`9<+ibY&b`3P|f)V+kb-X<<5F5+VlcM@zaNood zPWc5;lzsCWG)wD`)#9uGy-iU$EA;6JSnpif2l6(ol*G*zk?c_t7duTWM!GnGS`g|g zD^|O9Qq08lnPRnT+S}V$^d(fe2G3kT*Mf1FglDFa;(F8_saS(TePSov^v);S{NR;62%sgr)|yf^&jikY3Ij-tf~Pa-tdm(YnG@19V_b8|#JmS9GS5hB zqi#nP-lln?_%=<+V(+_UL%Xx~t(2}E + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-25 05:42:57Functions:8988.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.cpp +
60.0%60.0%
+ 		60.0 %15 / 2583.3 %5 / 6
<unnamed>60.0 %15 / 2583.3 %5 / 6
line_operations.cpp +
66.7%66.7%
+ 		66.7 %16 / 24100.0 %3 / 3
<unnamed>66.7 %16 / 24100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/index-detail-sort-l.html b/mrs_lib/src/safety_zone/index-detail-sort-l.html new file mode 100644 index 0000000000..ec7b3a2850 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-25 05:42:57Functions:8988.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.cpp +
60.0%60.0%
+ 		60.0 %15 / 2583.3 %5 / 6
<unnamed>60.0 %15 / 2583.3 %5 / 6
line_operations.cpp +
66.7%66.7%
+ 		66.7 %16 / 24100.0 %3 / 3
<unnamed>66.7 %16 / 24100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/index-detail.html b/mrs_lib/src/safety_zone/index-detail.html new file mode 100644 index 0000000000..fe1fd16bd4 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-25 05:42:57Functions:8988.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
line_operations.cpp +
66.7%66.7%
+ 		66.7 %16 / 24100.0 %3 / 3
<unnamed>66.7 %16 / 24100.0 %3 / 3
safety_zone.cpp +
60.0%60.0%
+ 		60.0 %15 / 2583.3 %5 / 6
<unnamed>60.0 %15 / 2583.3 %5 / 6
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/index-sort-f.html b/mrs_lib/src/safety_zone/index-sort-f.html new file mode 100644 index 0000000000..48beefdae7 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-25 05:42:57Functions:8988.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.cpp +
60.0%60.0%
+ 		60.0 %15 / 2583.3 %5 / 6
line_operations.cpp +
66.7%66.7%
+ 		66.7 %16 / 24100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/index-sort-l.html b/mrs_lib/src/safety_zone/index-sort-l.html new file mode 100644 index 0000000000..4c035a4751 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-25 05:42:57Functions:8988.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
safety_zone.cpp +
60.0%60.0%
+ 		60.0 %15 / 2583.3 %5 / 6
line_operations.cpp +
66.7%66.7%
+ 		66.7 %16 / 24100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/index.html b/mrs_lib/src/safety_zone/index.html new file mode 100644 index 0000000000..c91040106e --- /dev/null +++ b/mrs_lib/src/safety_zone/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-25 05:42:57Functions:8988.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_operations.cpp +
66.7%66.7%
+ 		66.7 %16 / 24100.0 %3 / 3
safety_zone.cpp +
60.0%60.0%
+ 		60.0 %15 / 2583.3 %5 / 6
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html b/mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html new file mode 100644 index 0000000000..f139d8f569 --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
getScale(Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>)580
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)612
mrs_lib::sectionIntersect(Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>)612
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/line_operations.cpp.func.html b/mrs_lib/src/safety_zone/line_operations.cpp.func.html new file mode 100644 index 0000000000..c3fcd20691 --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
getScale(Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>)580
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)612
mrs_lib::sectionIntersect(Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>)612
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/line_operations.cpp.gcov.frameset.html b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.frameset.html new file mode 100644 index 0000000000..e4725be416 --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/safety_zone/line_operations.cpp.gcov.html b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.html new file mode 100644 index 0000000000..df951d85ea --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.html @@ -0,0 +1,143 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/safety_zone/line_operations.h>
+       2             : 
+       3             : /* getScale() //{ */
+       4             : 
+       5         580 : static double getScale(Eigen::RowVector2d start, Eigen::RowVector2d vector, Eigen::RowVector2d point) {
+       6             :   // Returns the scalar that produces: start + scale * vector = point
+       7             :   // Assuming that such scalar exists
+       8         580 :   return vector(0) != 0 ? (point(0) - start(0)) / vector(0) : (point(1) - start(1)) / vector(1);
+       9             : }
+      10             : 
+      11             : //}
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15         612 : Intersection::Intersection(bool intersect, bool parallel, Eigen::RowVector2d point) : point(std::move(point)), parallel(parallel), intersect(intersect){}
+      16             : 
+      17             : /* sectionIntersect() //{ */
+      18             : 
+      19         612 : Intersection sectionIntersect(Eigen::RowVector2d start1, Eigen::RowVector2d end1, Eigen::RowVector2d start2, Eigen::RowVector2d end2) {
+      20         612 :   Eigen::RowVector2d vector1 = end1 - start1;
+      21         612 :   Eigen::RowVector2d vector2 = end2 - start2;
+      22             : 
+      23             :   // x - cross product (in 2d is just a scalar of z)
+      24             :   // start1 + scale1 * vector1 = start2 + scale2 * vector2  // x vector2
+      25             :   // scale1 * vector1 x vector2 = (start2 - start1) x vector2
+      26             : 
+      27             :   // cross product
+      28         612 :   double cross            = vector1(0) * vector2(1) - vector1(1) * vector2(0);
+      29         612 :   double difference_cross = (start2(0) - start1(0)) * vector2(1) - (start2(1) - start1(1)) * vector2(0);
+      30             : 
+      31         612 :   if (cross == 0) {
+      32             :     // are parallel
+      33          32 :     if (difference_cross != 0)
+      34          32 :       return Intersection{false, true};
+      35             : 
+      36             :     // are collinear
+      37           0 :     double start2Scale = getScale(start1, vector1, start2);
+      38           0 :     if (0 <= start2Scale && start2Scale <= 1)
+      39           0 :       return Intersection{true, true, start2};
+      40           0 :     double end2Scale = getScale(start1, vector1, end2);
+      41           0 :     if (0 <= end2Scale && end2Scale <= 1)
+      42           0 :       return Intersection{true, true, end2};
+      43             : 
+      44           0 :     return Intersection{false, true};
+      45             :   }
+      46             : 
+      47         580 :   double             scale1 = difference_cross / cross;
+      48         580 :   Eigen::RowVector2d point  = start1 + scale1 * vector1;
+      49         580 :   double             scale2 = getScale(start2, vector2, point);
+      50             : 
+      51         580 :   if (0 <= scale1 && scale1 <= 1 && 0 <= scale2 && scale2 <= 1) {
+      52           0 :     return Intersection{true, false, point};
+      53             :   }
+      54         580 :   return Intersection{false, false};
+      55             : }
+      56             : 
+      57             : //}
+      58             : 
+      59             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/line_operations.cpp.gcov.overview.html b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.overview.html new file mode 100644 index 0000000000..0299c5c2d4 --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.overview.html @@ -0,0 +1,35 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/safety_zone/line_operations.cpp.gcov.png b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..f17bea8867e5900f80693d9429a08dd5c1c6f437 GIT binary patch literal 465 zcmV;?0WSWDP)x`S;$& zh!6sV;6o2ji*1r0-bIVAif(bpzgG0Gm;f;b+KCy$k6* zs4m;HHi$ik5mX|ecdHlV%1G1G#Q0wdsxjOFHw9s>yDgx~&V~I2Q7j6pwWdbYR+}PA z)a9H3)GF?29@(tQ}DV%D5I>#>bITI#a<(uasiqwRl`8#4}cAWZu~^8nKxqW5`|+ zD{t}~@5JhwJTDz@U9X8EsL|jQT1Q;3&qOhbB&eEjshWj7h-17`uxa4>$3$FNY>9oBsNWudC5U^nv{V0@Ny(PO3(N00000NkvXX Hu0mjfFxJW; literal 0 HcmV?d00001 diff --git a/mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html b/mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html new file mode 100644 index 0000000000..0f792c5f6b --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
polygon.cpp +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
<unnamed>40.8 %42 / 10350.0 %4 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html b/mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html new file mode 100644 index 0000000000..6e4757f420 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
polygon.cpp +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
<unnamed>40.8 %42 / 10350.0 %4 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/index-detail.html b/mrs_lib/src/safety_zone/polygon/index-detail.html new file mode 100644 index 0000000000..6c2ae10c9c --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
polygon.cpp +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
<unnamed>40.8 %42 / 10350.0 %4 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/index-sort-f.html b/mrs_lib/src/safety_zone/polygon/index-sort-f.html new file mode 100644 index 0000000000..3fb10fa1a1 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
polygon.cpp +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/index-sort-l.html b/mrs_lib/src/safety_zone/polygon/index-sort-l.html new file mode 100644 index 0000000000..09e44abf41 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
polygon.cpp +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/index.html b/mrs_lib/src/safety_zone/polygon/index.html new file mode 100644 index 0000000000..86d77cc248 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
polygon.cpp +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html new file mode 100644 index 0000000000..6dc6849cda --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Polygon::inflateSelf(double)0
mrs_lib::Polygon::isClockwise()0
mrs_lib::lineIntersectGivenVector(Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>)0
mrs_lib::movePoint(Eigen::Matrix<double, 1, 2, 1, 1, 2>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2>)0
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)73
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)153
mrs_lib::Polygon::isPointInside(double, double)10339
mrs_lib::Polygon::getPointMessageVector(double)22516
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html new file mode 100644 index 0000000000..105ddbdacb --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Polygon::inflateSelf(double)0
mrs_lib::Polygon::isClockwise()0
mrs_lib::Polygon::isPointInside(double, double)10339
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)153
mrs_lib::Polygon::getPointMessageVector(double)22516
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)73
mrs_lib::lineIntersectGivenVector(Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>)0
mrs_lib::movePoint(Eigen::Matrix<double, 1, 2, 1, 1, 2>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2>)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.frameset.html b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.frameset.html new file mode 100644 index 0000000000..c64c98491c --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html new file mode 100644 index 0000000000..4c6ca6da59 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html @@ -0,0 +1,290 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-25 05:42:57Functions:4850.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/safety_zone/polygon.h>
+       2             : #include <mrs_lib/safety_zone/line_operations.h>
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             : 
+       7             : /* Polygon() //{ */
+       8             : 
+       9          73 : Polygon::Polygon(const Eigen::MatrixXd vertices) : vertices(vertices) {
+      10             : 
+      11          73 :   if (vertices.cols() != 2) {
+      12           0 :     ROS_WARN("(Polygon) The supplied polygon has to have 2 cols. It has %lu.", vertices.cols());
+      13           0 :     throw WrongNumberOfColumns();
+      14             :   }
+      15          73 :   if (vertices.rows() < 3) {
+      16           0 :     ROS_WARN("(Polygon) The supplied polygon has to have at least 3 vertices. It has %lu.", vertices.rows());
+      17           0 :     throw WrongNumberOfVertices();
+      18             :   }
+      19             : 
+      20          73 :   Eigen::RowVector2d edge1;
+      21          73 :   Eigen::RowVector2d edge2 = vertices.row(1) - vertices.row(0);
+      22         365 :   for (int i = 0; i < vertices.rows(); ++i) {
+      23         292 :     edge1 = edge2;
+      24         292 :     edge2 = vertices.row((i + 2) % vertices.rows()) - vertices.row((i + 1) % vertices.rows());
+      25             : 
+      26         292 :     if (edge1(0) == 0 && edge1(1) == 0) {
+      27           0 :       ROS_WARN("(Polygon) Polygon edge %d is of length zero.", i);
+      28           0 :       throw ExtraVertices();
+      29             :     }
+      30         292 :     if (edge1(0) * edge2(1) == edge1(1) * edge2(0)) {
+      31           0 :       ROS_WARN("(Polygon) Adjacent Polygon edges at vertice %d are collinear.", (int)((i + 1) % vertices.rows()));
+      32           0 :       throw ExtraVertices();
+      33             :     }
+      34             :   }
+      35          73 : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* isPointInside() //{ */
+      40             : 
+      41       10339 : bool Polygon::isPointInside(const double px, const double py) {
+      42             : 
+      43       10339 :   int count = 0;
+      44             : 
+      45             :   // Cast a horizontal ray and see how many times it intersects all the edges
+      46       51695 :   for (int i = 0; i < vertices.rows(); ++i) {
+      47       41356 :     double v1x = vertices(i, 0);
+      48       41356 :     double v1y = vertices(i, 1);
+      49       41356 :     double v2x = vertices((i + 1) % vertices.rows(), 0);
+      50       41356 :     double v2y = vertices((i + 1) % vertices.rows(), 1);
+      51             : 
+      52       41356 :     if (v1y > py && v2y > py)
+      53       10261 :       continue;
+      54       31095 :     if (v1y <= py && v2y <= py)
+      55       10573 :       continue;
+      56       20522 :     double intersect_x    = (v1y == v2y) ? v1x : (py - v1y) * (v2x - v1x) / (v2y - v1y) + v1x;
+      57       20522 :     bool   does_intersect = intersect_x > px;
+      58       20522 :     if (does_intersect)
+      59       10261 :       ++count;
+      60             :   }
+      61             : 
+      62       10339 :   return count % 2;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* doesSectionIntersect() //{ */
+      68             : 
+      69         153 : bool Polygon::doesSectionIntersect(const double startX, const double startY, const double endX, const double endY) {
+      70             : 
+      71         153 :   Eigen::RowVector2d start{startX, startY};
+      72         153 :   Eigen::RowVector2d end{endX, endY};
+      73             : 
+      74         765 :   for (int i = 0; i < vertices.rows(); ++i) {
+      75             : 
+      76         612 :     Eigen::RowVector2d edgeStart = vertices.row(i);
+      77         612 :     Eigen::RowVector2d edgeEnd   = vertices.row((i + 1) % vertices.rows());
+      78             : 
+      79         612 :     if (sectionIntersect(start, end, edgeStart, edgeEnd).intersect) {
+      80           0 :       return true;
+      81             :     }
+      82             :   }
+      83             : 
+      84         153 :   return false;
+      85             : }
+      86             : 
+      87             : //}
+      88             : 
+      89             : /* isClockwise() //{ */
+      90             : 
+      91           0 : bool Polygon::isClockwise() {
+      92             : 
+      93           0 :   int edgeIndex = 0;
+      94           0 :   if (vertices(0, 1) == vertices(1, 1))
+      95           0 :     edgeIndex = 1;
+      96             : 
+      97             :   // get the midPoint of first edge
+      98           0 :   Eigen::RowVector2d center = (vertices.row(edgeIndex) + vertices.row(edgeIndex + 1)) / 2;
+      99             : 
+     100             :   // count the intersections for a horizontal ray starting at center
+     101           0 :   int intersection_count = 0;
+     102           0 :   for (int i = 0; i < vertices.rows(); ++i) {
+     103           0 :     if (i == edgeIndex)
+     104           0 :       continue;
+     105             : 
+     106           0 :     Eigen::MatrixXd edge(2, 2);
+     107           0 :     edge.row(0) = vertices.row(i);
+     108           0 :     edge.row(1) = vertices.row((i + 1) % vertices.rows());
+     109             : 
+     110           0 :     if ((edge(0, 1) <= center(1) && edge(1, 1) <= center(1)) || (edge(0, 1) > center(1) && edge(1, 1) > center(1)))
+     111           0 :       continue;
+     112           0 :     double intersect_x = edge(0, 0) + (edge(1, 0) - edge(0, 0)) / (edge(1, 1) - edge(0, 1)) * (center(1) - edge(0, 1));
+     113           0 :     if (intersect_x >= center(0))
+     114           0 :       ++intersection_count;
+     115             :   }
+     116             : 
+     117             :   // reverse depending on the direction of edge
+     118           0 :   bool clockwise = intersection_count % 2;
+     119           0 :   if (vertices(edgeIndex + 1, 1) < vertices(edgeIndex, 0))
+     120           0 :     clockwise = !clockwise;
+     121           0 :   return clockwise;
+     122             : }
+     123             : 
+     124             : //}
+     125             : 
+     126             : /* movePoint() //{ */
+     127             : 
+     128           0 : static Eigen::RowVector2d movePoint(Eigen::RowVector2d vector, double scale, Eigen::RowVector2d point) {
+     129             : 
+     130           0 :   double             length         = sqrt(vector(0) * vector(0) + vector(1) * vector(1));
+     131           0 :   Eigen::RowVector2d unitary_vector = vector / length;
+     132           0 :   return point + scale * unitary_vector;
+     133             : }
+     134             : 
+     135             : //}
+     136             : 
+     137             : /* lineIntersectGivenVector() //{ */
+     138             : 
+     139           0 : static Intersection lineIntersectGivenVector(Eigen::RowVector2d start1, Eigen::RowVector2d vector1, Eigen::RowVector2d start2, Eigen::RowVector2d vector2) {
+     140             : 
+     141           0 :   double cross            = vector1(0) * vector2(1) - vector1(1) * vector2(0);
+     142           0 :   double difference_cross = (start2(0) - start1(0)) * vector2(1) - (start2(1) - start1(1)) * vector1(0);
+     143             : 
+     144           0 :   if (cross == 0) {
+     145           0 :     if (difference_cross == 0) {
+     146           0 :       return Intersection{true, true, start1};
+     147             :     }
+     148           0 :     return Intersection{false, true};
+     149             :   }
+     150             : 
+     151           0 :   double             scale1 = difference_cross / cross;
+     152           0 :   Eigen::RowVector2d point  = start1 + scale1 * vector1;
+     153           0 :   return Intersection{true, false, point};
+     154             : }
+     155             : 
+     156             : //}
+     157             : 
+     158             : /* inflateSelf() //{ */
+     159             : 
+     160           0 : void Polygon::inflateSelf(double amount) {
+     161             : 
+     162             :   // Also supports negative numbers
+     163           0 :   Eigen::MatrixXd result{vertices.rows(), vertices.cols()};
+     164             : 
+     165           0 :   bool clockwise = isClockwise();
+     166           0 :   amount         = (clockwise ? 1 : -1) * amount;
+     167             : 
+     168           0 :   for (int i = 0; i < vertices.rows(); ++i) {
+     169           0 :     Eigen::RowVector2d pPrev    = vertices.row((i + vertices.rows() - 1) % vertices.rows());
+     170           0 :     Eigen::RowVector2d pCurrent = vertices.row(i);
+     171           0 :     Eigen::RowVector2d pNext    = vertices.row((i + 1) % vertices.rows());
+     172             : 
+     173           0 :     Eigen::RowVector2d vector1 = pCurrent - pPrev;
+     174           0 :     Eigen::RowVector2d vector2 = pNext - pCurrent;
+     175             :     // make them orthogonally counter-clockwise
+     176           0 :     Eigen::RowVector2d vectorOrthogonal1{-vector1(1), vector1(0)};
+     177           0 :     Eigen::RowVector2d vectorOrthogonal2{-vector2(1), vector2(0)};
+     178             :     // move the currentPoint in 2 directions
+     179           0 :     pPrev = movePoint(vectorOrthogonal1, amount, pCurrent);
+     180           0 :     pNext = movePoint(vectorOrthogonal2, amount, pCurrent);
+     181             : 
+     182             :     // add the intersection as a new point
+     183           0 :     result.row(i) = lineIntersectGivenVector(pPrev, vector1, pNext, vector2).point;
+     184             :   }
+     185             : 
+     186           0 :   vertices = result.replicate(result.rows(), result.cols());
+     187           0 : }
+     188             : 
+     189             : //}
+     190             : 
+     191             : /* getPointMessageVector() //{ */
+     192             : 
+     193       22516 : std::vector<geometry_msgs::Point> Polygon::getPointMessageVector(const double z) {
+     194       22516 :   std::vector<geometry_msgs::Point> points(vertices.rows());
+     195      112580 :   for (int i = 0; i < vertices.rows(); ++i) {
+     196       90064 :     points[i].x = vertices(i, 0);
+     197       90064 :     points[i].y = vertices(i, 1);
+     198       90064 :     points[i].z = z;
+     199             :   }
+     200             : 
+     201       22516 :   return points;
+     202             : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.overview.html b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.overview.html new file mode 100644 index 0000000000..9588a3df3c --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.overview.html @@ -0,0 +1,72 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.png b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c5adcdf99ff4cbdb4f4d66b7c0b94eb7d1a120a0 GIT binary patch literal 1057 zcmV++1m63JP)$b|L8UA<<9=Mz;iKKL zc85A%)j`nM+>C0sP4({7g2YB=->8c@2R2p;@?r$K>&=yT$KdT}Al0K?NiI0!a}48<^f9CF$uEE6&Xh67|26T@sH z7?{ReS$eS%EQ?keA8+y23pTP@Y7kj9GEBiagRZv3^p8S}k;{~ln*bK@=b`v9j>8O7x3!hsfSt!0@1=dGtW8oJBV+H5d4pTv>`(hy&)*zj( zSqc|y+@OqrDq`;y@=DY6ai)T1whFiix`T|!T^oY}^bsykVh{1lIoYnb$6|g3+q6ka zf*0^Cb!{i`6ica*0WU1XBl+wQrpE`8=bAYR#+>13r*0wuh;W)wx`L2EJsdUqdoucs z>751lyhip9?uS{z_63)vC3)O8SRs2%HVsZky;{@1C6OjH!|f)7T9W|!bqF70M!(0mQFJONCbVxa9@L zEt4xx?zIhNaw$wKa~b+pJI7w~4+MWHxn3%U!8@&3TdVdCd`UBUtwQS~TOSiNmh!ro zhWj-I!~o{6fy$(QU~vh+tVn6(DpJg)18-I(*e?%98jJGqM(GOikoso`#x5IzI(Vpy za$DmyH0}s$eAkoe(FOy=H$Lqep^itkAk*Pwbrwp@aM;Wlm9T9LTBpIkM9swtu<$+6 z$R<*@dtRpp48Ije%?LqZv5O4oNNrewuL-@oAL{rSI2&^ b{<+`}Uto53>9?6+00000NkvXXu0mjfHe=(^ literal 0 HcmV?d00001 diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html b/mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html new file mode 100644 index 0000000000..b6d832bc2a --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)73
mrs_lib::SafetyZone::~SafetyZone()73
mrs_lib::SafetyZone::isPathValid(double, double, double, double)153
mrs_lib::SafetyZone::isPointValid(double, double)10339
mrs_lib::SafetyZone::getBorder()11258
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.func.html b/mrs_lib/src/safety_zone/safety_zone.cpp.func.html new file mode 100644 index 0000000000..9270e57371 --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SafetyZone::isPathValid(double, double, double, double)153
mrs_lib::SafetyZone::isPointValid(double, double)10339
mrs_lib::SafetyZone::getBorder()11258
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)73
mrs_lib::SafetyZone::~SafetyZone()73
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.frameset.html b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.frameset.html new file mode 100644 index 0000000000..3322088862 --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html new file mode 100644 index 0000000000..f996210413 --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/safety_zone/safety_zone.h>
+       2             : #include <mrs_lib/safety_zone/line_operations.h>
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             : /* SafetyZone() //{ */
+       7             : 
+       8           0 : SafetyZone::SafetyZone(mrs_lib::Polygon border) {
+       9           0 :   outerBorder = new Polygon(border);
+      10           0 : }
+      11             : 
+      12             : //}
+      13             : 
+      14             : /* SafetyZone() //{ */
+      15             : 
+      16         146 : SafetyZone::~SafetyZone() {
+      17          73 :   delete outerBorder;
+      18          73 : }
+      19             : 
+      20             : //}
+      21             : 
+      22             : /* SafetyZone() //{ */
+      23             : 
+      24          73 : SafetyZone::SafetyZone(const Eigen::MatrixXd& outerBorderMatrix) {
+      25             : 
+      26             :   try {
+      27          73 :     outerBorder = new Polygon(outerBorderMatrix);
+      28             :   }
+      29           0 :   catch (const Polygon::WrongNumberOfVertices&) {
+      30           0 :     throw BorderError();
+      31             :   }
+      32           0 :   catch (const Polygon::WrongNumberOfColumns&) {
+      33           0 :     throw BorderError();
+      34             :   }
+      35           0 :   catch (const Polygon::ExtraVertices&) {
+      36           0 :     throw BorderError();
+      37             :   }
+      38          73 : }
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* isPointValid() //{ */
+      43             : 
+      44       10339 : bool SafetyZone::isPointValid(const double px, const double py) {
+      45             : 
+      46       10339 :   if (!outerBorder->isPointInside(px, py)) {
+      47          78 :     return false;
+      48             :   }
+      49             : 
+      50       10261 :   return true;
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* isPathValid() //{ */
+      56             : 
+      57         153 : bool SafetyZone::isPathValid(const double p1x, const double p1y, const double p2x, const double p2y) {
+      58             : 
+      59         153 :   if (outerBorder->doesSectionIntersect(p1x, p1y, p2x, p2y)) {
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63         153 :   return true;
+      64             : }
+      65             : 
+      66             : //}
+      67             : 
+      68             : /* getBorder() //{ */
+      69             : 
+      70       11258 : Polygon SafetyZone::getBorder() {
+      71       11258 :   return *outerBorder;
+      72             : }
+      73             : 
+      74             : //}
+      75             : 
+      76             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html new file mode 100644 index 0000000000..516836160f --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html @@ -0,0 +1,39 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..167b4f7e497806e1c5538d1ea552833672a06ad0 GIT binary patch literal 380 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1-!VDxiv~}x%lth3}i0l9V|5pLQ^7pZ^ul_SI ztOAOIsdL_&mjcUg-y18yaI7 z%{TmBAEdP6g!??PYWMlaW!d{%T5MX_xDAgi5UEU>#?=(=ooAXfbwgx{^ptPA_pfEX zr8Q=~Zx;(p%CC$O zP7U7LAJccMt@r5KDfW90#J-#3(dd47R^nEljR%4XE7Kywv;{-LPkx;e_>ezFUC{o? zF6FZ(B4^i4cF>!}Zu))uYmWY?qhdFgoD=+88s=@YPEz7t@m-}D5rcW!T!tNP+a@o% z@J`_IIdj*CXR4B|CzhW5w0@=7b<>`Dovt!d$M*P_C!JL*md<&;soTa&apCeG>;_zJ VEeW!JlYl|R;OXk;vd$@?2>{; + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
scope_timer.cpp +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
<unnamed>14.1 %20 / 14250.0 %5 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/index-detail-sort-l.html b/mrs_lib/src/scope_timer/index-detail-sort-l.html new file mode 100644 index 0000000000..854d546dbe --- /dev/null +++ b/mrs_lib/src/scope_timer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
scope_timer.cpp +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
<unnamed>14.1 %20 / 14250.0 %5 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/index-detail.html b/mrs_lib/src/scope_timer/index-detail.html new file mode 100644 index 0000000000..1e280b06e4 --- /dev/null +++ b/mrs_lib/src/scope_timer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
scope_timer.cpp +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
<unnamed>14.1 %20 / 14250.0 %5 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/index-sort-f.html b/mrs_lib/src/scope_timer/index-sort-f.html new file mode 100644 index 0000000000..70d450c0e0 --- /dev/null +++ b/mrs_lib/src/scope_timer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
scope_timer.cpp +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/index-sort-l.html b/mrs_lib/src/scope_timer/index-sort-l.html new file mode 100644 index 0000000000..e47a88f244 --- /dev/null +++ b/mrs_lib/src/scope_timer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
scope_timer.cpp +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/index.html b/mrs_lib/src/scope_timer/index.html new file mode 100644 index 0000000000..7d43833274 --- /dev/null +++ b/mrs_lib/src/scope_timer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
scope_timer.cpp +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html b/mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html new file mode 100644 index 0000000000..7217ec216d --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timer - scope_timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ScopeTimer::getLifetime()0
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&, bool, std::shared_ptr<mrs_lib::ScopeTimerLogger>)0
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_lib::ScopeTimer::time_point const&, ros::Duration const&, bool, std::shared_ptr<mrs_lib::ScopeTimerLogger>)0
mrs_lib::ScopeTimerLogger::log(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&)0
mrs_lib::ScopeTimerLogger::log(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&)0
mrs_lib::ScopeTimerLogger::ScopeTimerLogger(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool, int)658
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()658
mrs_lib::ScopeTimer::checkpoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4084930
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_lib::ScopeTimerLogger>, bool)5214405
mrs_lib::ScopeTimer::~ScopeTimer()5216663
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.func.html b/mrs_lib/src/scope_timer/scope_timer.cpp.func.html new file mode 100644 index 0000000000..69e4f0884d --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timer - scope_timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ScopeTimer::checkpoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4084930
mrs_lib::ScopeTimer::getLifetime()0
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&, bool, std::shared_ptr<mrs_lib::ScopeTimerLogger>)0
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_lib::ScopeTimer::time_point const&, ros::Duration const&, bool, std::shared_ptr<mrs_lib::ScopeTimerLogger>)0
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_lib::ScopeTimerLogger>, bool)5214405
mrs_lib::ScopeTimer::~ScopeTimer()5216663
mrs_lib::ScopeTimerLogger::log(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&)0
mrs_lib::ScopeTimerLogger::log(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&, std::chrono::time_point<std::chrono::_V2::steady_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&)0
mrs_lib::ScopeTimerLogger::ScopeTimerLogger(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool, int)658
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()658
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.frameset.html b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.frameset.html new file mode 100644 index 0000000000..56e3d2ae0a --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html new file mode 100644 index 0000000000..87ef54af09 --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html @@ -0,0 +1,345 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timer - scope_timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-25 05:42:57Functions:51050.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/scope_timer.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6             : // | --------------------- ScopeTimerLogger --------------------- |
+       7             : 
+       8             : /*//{ ScopeTimerLogger constructor */
+       9         658 : ScopeTimerLogger::ScopeTimerLogger(const std::string& logfile, const bool enable_logging, const int log_precision)
+      10         658 :     : _logging_enabled_(enable_logging), _log_filepath_(logfile) {
+      11             : 
+      12         658 :   if (!_logging_enabled_) {
+      13         658 :     return;
+      14           0 :   } else if (logfile.empty()) {
+      15           0 :     _logging_enabled_ = false;
+      16           0 :     ROS_INFO("[%s] ScopeTimerLogger: Logfile path not specified. Skipping logging to file.", ros::this_node::getName().c_str());
+      17           0 :     return;
+      18             :   }
+      19           0 :   _should_log_ = true;
+      20             : 
+      21             :   try {
+      22           0 :     std::scoped_lock lock(_mutex_logstream_);
+      23             : 
+      24           0 :     _logstream_ = std::ofstream(logfile, std::ios_base::out | std::ios_base::trunc);
+      25             : 
+      26           0 :     if (!_logstream_.is_open()) {
+      27           0 :       _logging_enabled_ = false;
+      28           0 :       ROS_ERROR("[%s]: Scope timer failed to create log file with path (%s). Skipping logging.", ros::this_node::getName().c_str(), logfile.c_str());
+      29           0 :       return;
+      30             :     }
+      31             : 
+      32           0 :     _logstream_ << std::fixed << std::setprecision(log_precision);
+      33           0 :     _logstream_ << "#scope,label_from,label_to,sec_start,sec_end,sec_duration" << std::endl;
+      34           0 :     _logging_enabled_ = true;
+      35             :   }
+      36           0 :   catch (...) {
+      37           0 :     ROS_ERROR("[%s]: Scope timer logger could not open logger file at: %s. Skipping logging.", ros::this_node::getName().c_str(), logfile.c_str());
+      38           0 :     return;
+      39             :   }
+      40             : 
+      41           0 :   ROS_INFO("[%s]: Scope timer logger path: %s.", ros::this_node::getName().c_str(), logfile.c_str());
+      42             : }
+      43             : /*//}*/
+      44             : 
+      45             : /*//{ ScopeTimerLogger destructor */
+      46         658 : ScopeTimerLogger::~ScopeTimerLogger() {
+      47         658 :   if (_logging_enabled_) {
+      48           0 :     ROS_DEBUG("[%s]: ScopeTimerLogger: closing logstream.", ros::this_node::getName().c_str());
+      49           0 :     _logstream_.close();
+      50             :   }
+      51         658 : }
+      52             : /*//}*/
+      53             : 
+      54             : /*//{ ScopeTimerLogger::log() */
+      55           0 : void ScopeTimerLogger::log(const std::string& scope, const chrono_tp& time_start, const chrono_tp& time_end) {
+      56           0 :   log(scope, "", "", time_start, time_end);
+      57           0 : }
+      58             : 
+      59           0 : void ScopeTimerLogger::log(const std::string& scope, const std::string& label_from, const std::string& label_to, const chrono_tp& time_start,
+      60             :                            const chrono_tp& time_end) {
+      61             : 
+      62           0 :   if (!_logging_enabled_) {
+      63           0 :     return;
+      64             :   }
+      65             : 
+      66           0 :   const std::chrono::duration<double> duration_start = std::chrono::duration_cast<std::chrono::duration<double>>(time_start.time_since_epoch());
+      67           0 :   const std::chrono::duration<double> duration_end   = std::chrono::duration_cast<std::chrono::duration<double>>(time_end.time_since_epoch());
+      68           0 :   const std::chrono::duration<double> duration_total = std::chrono::duration_cast<std::chrono::duration<double>>(time_end - time_start);
+      69             : 
+      70             :   {
+      71           0 :     std::scoped_lock lock(_mutex_logstream_);
+      72           0 :     _logstream_ << scope.c_str() << "," << label_from.c_str() << "," << label_to.c_str() << "," << duration_start.count() << "," << duration_end.count() << ","
+      73           0 :                 << duration_total.count() << std::endl;
+      74             :   }
+      75             : }
+      76             : /*//}*/
+      77             : 
+      78             : // | ------------------------ ScopeTimer ------------------------ |
+      79             : 
+      80             : std::unordered_map<std::string, ros::Time> ScopeTimer::last_print_times;
+      81             : 
+      82             : /* ScopeTimer constructor //{ */
+      83             : 
+      84           0 : ScopeTimer::ScopeTimer(const std::string& label, const ros::Duration& throttle_period, const bool enable,
+      85           0 :                        const std::shared_ptr<ScopeTimerLogger> scope_timer_logger)
+      86           0 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(throttle_period), _logger_(scope_timer_logger) {
+      87             : 
+      88           0 :   checkpoints.push_back(time_point("timer start"));
+      89             : 
+      90           0 :   ROS_DEBUG("[%s] Scope timer started, label: %s", ros::this_node::getName().c_str(), label.c_str());
+      91           0 : }
+      92             : 
+      93           0 : ScopeTimer::ScopeTimer(const std::string& label, const time_point& tp0, const ros::Duration& throttle_period, const bool enable,
+      94           0 :                        const std::shared_ptr<ScopeTimerLogger> scope_timer_logger)
+      95           0 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(throttle_period), _logger_(scope_timer_logger) {
+      96             : 
+      97           0 :   checkpoints.push_back(tp0);
+      98           0 :   checkpoints.push_back(time_point("timer start"));
+      99             : 
+     100           0 :   ROS_DEBUG("[%s] Scope timer started with file logger (label: %s).", ros::this_node::getName().c_str(), label.c_str());
+     101           0 : }
+     102             : 
+     103     5214405 : ScopeTimer::ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable)
+     104     5214405 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(ros::Duration(0.0)), _logger_(scope_timer_logger) {
+     105             : 
+     106     5203169 :   checkpoints.push_back(time_point("timer start"));
+     107             : 
+     108     5201655 :   ROS_DEBUG("[%s] Scope timer started with file logger (label: %s).", ros::this_node::getName().c_str(), label.c_str());
+     109     5201655 : }
+     110             : 
+     111             : //}
+     112             : 
+     113             : /* ScopeTimer::checkpoint() //{ */
+     114             : 
+     115     4084930 : void ScopeTimer::checkpoint(const std::string& label) {
+     116             : 
+     117     4084930 :   if (_enable_print_or_log) {
+     118           0 :     checkpoints.push_back(time_point(label));
+     119             :   }
+     120     4084930 : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* ScopeTimer::getLifetime() //{ */
+     125             : 
+     126           0 : float ScopeTimer::getLifetime() {
+     127           0 :   const auto lifetime_us = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - checkpoints.at(0).chrono_time).count();
+     128           0 :   return lifetime_us / 1000.0f;
+     129             : }
+     130             : 
+     131             : //}
+     132             : 
+     133             : /* ScopeTimer destructor //{ */
+     134             : 
+     135    15644818 : ScopeTimer::~ScopeTimer() {
+     136             : 
+     137     5216663 :   if (!_enable_print_or_log) {
+     138     5216736 :     return;
+     139             :   }
+     140             : 
+     141          13 :   const auto chrono_end_time = std::chrono::steady_clock::now();
+     142           0 :   const auto ros_end_time    = ros::Time::now();
+     143             : 
+     144             :   // if throttling is enabled, check time of last print and only print if applicable
+     145           0 :   if (!_throttle_period_.isZero()) {
+     146           0 :     bool       do_print = false;
+     147           0 :     const auto last_it  = last_print_times.find(_timer_label_);
+     148             :     // if this is the first print of this ScopeTimer
+     149           0 :     if (last_it == last_print_times.end()) {
+     150           0 :       do_print = true;
+     151           0 :       last_print_times.emplace(_timer_label_, ros_end_time);
+     152             :     } else {
+     153             :       // if this ScopeTimer was already printed, check how long ago
+     154           0 :       ros::Time& last_print_time = last_it->second;
+     155           0 :       if (ros_end_time - last_print_time > _throttle_period_) {
+     156             :         // if it was long ago enough, print again and update the last print time
+     157           0 :         do_print        = true;
+     158           0 :         last_print_time = ros_end_time;
+     159             :       }
+     160             :     }
+     161             : 
+     162           0 :     if (!do_print)
+     163           0 :       return;
+     164             :   }
+     165             : 
+     166             :   // If logger object exists and it should log (a path to a logger file was given by the user)
+     167           0 :   if (_logger_ && _logger_->shouldLog()) {
+     168             : 
+     169             :     // Enabled, if user sets the enable flag to true, a path to a logger file is given, and the logging stream was successfully opened
+     170           0 :     if (!_logger_->loggingEnabled()) {
+     171           0 :       return;
+     172             :     }
+     173             : 
+     174             :     // Log checkpoints, e.g., "SCOPE->checkpoint1;checkpoint1->checkpoint2"
+     175           0 :     std::string label_from = "";
+     176           0 :     for (size_t i = 1; i < checkpoints.size(); i++) {
+     177           0 :       const auto& checkpoint = checkpoints.at(i);
+     178           0 :       _logger_->log(_timer_label_, label_from, checkpoint.label, checkpoints.at(i - 1).chrono_time, checkpoint.chrono_time);
+     179           0 :       label_from = checkpoint.label;
+     180             :     }
+     181             : 
+     182             :     // Log entire scope from start to end
+     183           0 :     _logger_->log(_timer_label_, checkpoints.at(0).chrono_time, chrono_end_time);
+     184             : 
+     185             :   } else {
+     186             : 
+     187           0 :     checkpoints.push_back(time_point("scope end"));
+     188             : 
+     189           0 :     const int gap1 = 8;
+     190           0 :     const int gap2 = 8;
+     191           0 :     const int gap3 = 12;
+     192             : 
+     193           0 :     std::stringstream ss;
+     194           0 :     ss.precision(3);
+     195           0 :     ss << std::fixed;
+     196             : 
+     197           0 :     const char separator = ' ';
+     198             : 
+     199           0 :     int max_label_width = 5;
+     200           0 :     for (auto& el : checkpoints) {
+     201           0 :       el.label      = "(" + el.label + ")";
+     202           0 :       const int len = el.label.length();
+     203           0 :       if (len > max_label_width)
+     204           0 :         max_label_width = len;
+     205             :     }
+     206             : 
+     207           0 :     int width_label_column = 10;
+     208           0 :     for (unsigned long i = 1; i < checkpoints.size(); i++) {
+     209           0 :       const int len = (checkpoints.at(i).label + checkpoints.at(i - 1).label).length();
+     210           0 :       if (len > width_label_column)
+     211           0 :         width_label_column = len;
+     212             :     }
+     213           0 :     width_label_column += 7;
+     214             : 
+     215           0 :     ss << std::endl << std::left << std::setw(width_label_column) << std::setfill(separator) << " {label}";
+     216           0 :     ss << std::left << std::setw(gap1) << std::setfill(separator) << "";
+     217           0 :     ss << std::left << std::setw(gap2) << std::setfill(separator) << "{ROS time}";
+     218           0 :     ss << std::left << std::setw(gap3) << std::setfill(separator) << "";
+     219           0 :     ss << std::left << std::setw(gap2) << std::setfill(separator) << "{chrono time}" << std::endl;
+     220             : 
+     221           0 :     for (unsigned long i = 1; i < checkpoints.size(); i++) {
+     222             : 
+     223           0 :       const double ros_elapsed = (checkpoints.at(i).ros_time - checkpoints.at(i - 1).ros_time).toSec();
+     224           0 :       const int    ros_secs    = int(ros_elapsed);
+     225           0 :       const double ros_msecs   = std::fmod(ros_elapsed * 1000, 1000);
+     226             : 
+     227           0 :       const std::chrono::duration<double> chrono_elapsed = (checkpoints.at(i).chrono_time - checkpoints.at(i - 1).chrono_time);
+     228           0 :       const int                           chrono_secs    = int(chrono_elapsed.count());
+     229           0 :       const double                        chrono_msecs   = std::fmod(chrono_elapsed.count() * 1000, 1000);
+     230             : 
+     231           0 :       ss << std::left << std::setw(max_label_width) << std::setfill(separator) << checkpoints.at(i - 1).label;
+     232           0 :       ss << " -> ";
+     233           0 :       ss << std::left << std::setw(max_label_width) << std::setfill(separator) << checkpoints.at(i).label;
+     234             : 
+     235           0 :       ss << std::right << std::setw(gap1) << std::setfill(separator) << ros_secs << std::setw(0) << "s";
+     236           0 :       ss << std::right << std::setw(gap2) << std::setfill(separator) << ros_msecs << std::setw(0) << "ms";
+     237           0 :       ss << std::right << std::setw(gap3) << std::setfill(separator) << chrono_secs << std::setw(0) << "s";
+     238           0 :       ss << std::right << std::setw(gap2) << std::setfill(separator) << chrono_msecs << std::setw(0) << "ms" << std::endl;
+     239             :     }
+     240             : 
+     241           0 :     const double ros_elapsed = (ros_end_time - checkpoints.at(0).ros_time).toSec();
+     242           0 :     const int    ros_secs    = int(ros_elapsed);
+     243           0 :     const double ros_msecs   = std::fmod(ros_elapsed * 1000, 1000);
+     244             : 
+     245           0 :     const std::chrono::duration<double> chrono_elapsed = (chrono_end_time - checkpoints.at(0).chrono_time);
+     246           0 :     const int                           chrono_secs    = int(chrono_elapsed.count());
+     247           0 :     const double                        chrono_msecs   = std::fmod(chrono_elapsed.count() * 1000, 1000);
+     248             : 
+     249           0 :     ss << std::left << std::setw(width_label_column) << std::setfill(separator) << "TOTAL TIME";
+     250           0 :     ss << std::right << std::setw(gap1) << std::setfill(separator) << ros_secs << std::setw(0) << "s";
+     251           0 :     ss << std::right << std::setw(gap2) << std::setfill(separator) << ros_msecs << std::setw(0) << "ms";
+     252           0 :     ss << std::right << std::setw(gap3) << std::setfill(separator) << chrono_secs << std::setw(0) << "s";
+     253           0 :     ss << std::right << std::setw(gap2) << std::setfill(separator) << chrono_msecs << std::setw(0) << "ms" << std::endl;
+     254             : 
+     255           0 :     ROS_INFO("[%s]: Scope timer [%s] finished!%s", ros::this_node::getName().c_str(), _timer_label_.c_str(), ss.str().c_str());
+     256             :   }
+     257     5214315 : }
+     258             : 
+     259             : //}
+     260             : 
+     261             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html new file mode 100644 index 0000000000..8fad4dc2df --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html @@ -0,0 +1,86 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1da9040a91ee22c35f65971da31ab5417532af9e GIT binary patch literal 1280 zcmV+b1^@bqP)#&hXlvB)tz2;Yvu&Pmt{=i}_=T zTK4#!0jGfVp*J=aI^6T6eR)Xs@1Sq%sQA^8*Zq0o;p;v!$8mOmczl4ELn-E9|9_q% z;1LlDjn-e63vUqbZAXl?85BNxI0keVJ8+l3d^(Fq0PGIPH7Y;=#PQs~KD1G((P#rH zyC+S%IXnH(sL^|-FX?7imoy6FdTxyxRg2Amj$R%KzqZez+g$$p^_^5`D?+93qK zYs^|5W=fn@*a${SjlzDJ>v}(O3Lo*smkGcN|D;nV{MH9rEwIw46n#m$u(6|Ih;yEA zCGpV7px_F9BYgEMqbA1(h?LjinB(Hg8+EvlTIKSs3sF3L4!B1xG8P-@pGYc8V+YI^ zV%<{*cjI)S+A9YmO}+%KYRnsZDmsKb!nhNMJ22_rarme#{0l2X4+gT-+F+5~!?Fj=DGlO=k&Q)p*h?zF#z-_y$Y$m6UquPZKlIUZT+t@LsW`v}~`k zD1NV@G3scDVtABf4-~`d$vnJ-;iT7+(o4a0gB}25c31Hz=f_LH`W+-S_vqd8ki++n zxm(^qPFi-_cx*Nin;LcZapq9wf_~NqGAG(7X(LF|$ivLDvcHZ;b@YT~!~QK+j$2sJ z_`XT-z7W4Lwk!`^M`Pq3d)E`wc37H?ElLW(R(B`P<1GTb9Oh-M5&ETWY$tW%RISez zkydVp0;%H?q0ykrH`-?~fo{)7zf%^`&@`8{+Ksv=-9=Sk##whzL@&D3UnYM->4Q}v z6mc}aQO_;`X{~#V7J({4GoOF@k-Re-+N!&*AMXKR7H$v=8=c*8vcvHmDwKte=SwT< z76<@X9CLm{8{){j7`^O=cNyTD-RpZY8{G->9vUz13~O^^vJ7m5L;cWZY|kZ03e%NVQy6W<(y4SQ{QVn9OHUEp(DQEr zC*A868)>Seu^Di#5qW1EZmWJoN*lOnNh8Hv$k;j1(YDpwx+x27+zA03mZ;S@x3q@r zX=7KTW2k|RVs{-)=7Gn6c@_$OgwS=K&0W?*P4`^9s_~gAgnN3?#@*9fltuS&eUs?X zNzNJH*tqlMs{)qTc)6*u1*B)1AJkZM3#YSI2ZdInO)X3wHJ3c~Da_#hqkoH3XcUi7 z96l<3*GA#H!orI-_L7wWZd*WUJWi(s>TN6+`>IbJ0>jlw>MUtowK2zGp)5Tb`X2C! zA70b}UiXrWGQh1-2Kf4Z&7}%(@0pEU;9L2u(kKIb@n<`&a4bp9D3{u4fohG{3UFa% qP!GN+fTR!puZHgqO{?jTDt`k-lcJw@c_}ym0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timeout_manager.cpp +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
<unnamed>79.0 %49 / 6281.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/index-detail-sort-l.html b/mrs_lib/src/timeout_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..20653ddfe7 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timeout_manager.cpp +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
<unnamed>79.0 %49 / 6281.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/index-detail.html b/mrs_lib/src/timeout_manager/index-detail.html new file mode 100644 index 0000000000..1725d3edac --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timeout_manager.cpp +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
<unnamed>79.0 %49 / 6281.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/index-sort-f.html b/mrs_lib/src/timeout_manager/index-sort-f.html new file mode 100644 index 0000000000..7321c85f55 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timeout_manager.cpp +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/index-sort-l.html b/mrs_lib/src/timeout_manager/index-sort-l.html new file mode 100644 index 0000000000..7bf7b923f9 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timeout_manager.cpp +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/index.html b/mrs_lib/src/timeout_manager/index.html new file mode 100644 index 0000000000..c794d00614 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timeout_manager.cpp +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..4886bc6a9b --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::TimeoutManager::change(unsigned long, ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)0
mrs_lib::TimeoutManager::lastReset(unsigned long)0
mrs_lib::TimeoutManager::pauseAll()2
mrs_lib::TimeoutManager::startAll(ros::Time const&)4
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::TimeoutManager(ros::NodeHandle const&, ros::Rate const&)88
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)91
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)91
mrs_lib::TimeoutManager::started(unsigned long)1562
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)20135
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)184599
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html new file mode 100644 index 0000000000..ac0e89c7e6 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)91
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)20135
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)184599
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)91
mrs_lib::TimeoutManager::change(unsigned long, ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)0
mrs_lib::TimeoutManager::started(unsigned long)1562
mrs_lib::TimeoutManager::pauseAll()2
mrs_lib::TimeoutManager::startAll(ros::Time const&)4
mrs_lib::TimeoutManager::lastReset(unsigned long)0
mrs_lib::TimeoutManager::TimeoutManager(ros::NodeHandle const&, ros::Rate const&)88
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.frameset.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..40489ce963 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html new file mode 100644 index 0000000000..5e9f03e33e --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html @@ -0,0 +1,186 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/timeout_manager.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6          88 :   TimeoutManager::TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate)
+       7          88 :     : m_last_id(0)
+       8             :   {
+       9          88 :     m_main_timer = nh.createTimer(update_rate, &TimeoutManager::main_timer_callback, this);
+      10          88 :   }
+      11             : 
+      12             : 
+      13          91 :   TimeoutManager::timeout_id_t TimeoutManager::registerNew(const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset, const bool oneshot, const bool autostart)
+      14             :   {
+      15         182 :     std::scoped_lock lck(m_mtx);
+      16          91 :     const auto new_id = m_timeouts.size();
+      17          91 :     const timeout_info_t new_info = {oneshot, autostart, callback, timeout, last_reset, last_reset};
+      18          91 :     m_timeouts.push_back(new_info);
+      19         182 :     return new_id;
+      20             :   }
+      21             : 
+      22      184599 :   void TimeoutManager::reset(const timeout_id_t id, const ros::Time& time)
+      23             :   {
+      24      184599 :     std::scoped_lock lck(m_mtx);
+      25      184599 :     m_timeouts.at(id).last_reset = time;
+      26      184599 :   }
+      27             : 
+      28           9 :   void TimeoutManager::pause(const timeout_id_t id)
+      29             :   {
+      30           9 :     std::scoped_lock lck(m_mtx);
+      31           9 :     m_timeouts.at(id).started = false;
+      32           9 :   }
+      33             : 
+      34          91 :   void TimeoutManager::start(const timeout_id_t id, const ros::Time& time)
+      35             :   {
+      36          91 :     std::scoped_lock lck(m_mtx);
+      37          91 :     m_timeouts.at(id).started = true;
+      38          91 :     m_timeouts.at(id).last_reset = time;
+      39          91 :   }
+      40             : 
+      41           2 :   void TimeoutManager::pauseAll()
+      42             :   {
+      43           4 :     std::scoped_lock lck(m_mtx);
+      44          10 :     for (auto& timeout_info : m_timeouts)
+      45           8 :       timeout_info.started = false;
+      46           2 :   }
+      47             : 
+      48           4 :   void TimeoutManager::startAll(const ros::Time& time)
+      49             :   {
+      50           8 :     std::scoped_lock lck(m_mtx);
+      51          20 :     for (auto& timeout_info : m_timeouts)
+      52             :     {
+      53          16 :       timeout_info.started = true;
+      54          16 :       timeout_info.last_reset = time;
+      55             :     }
+      56           4 :   }
+      57             : 
+      58           0 :   void TimeoutManager::change(const timeout_id_t id, const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset, const bool oneshot, const bool autostart)
+      59             :   {
+      60           0 :     std::scoped_lock lck(m_mtx);
+      61           0 :     auto& timeout_info = m_timeouts.at(id);
+      62           0 :     timeout_info.oneshot = oneshot;
+      63           0 :     timeout_info.started = autostart;
+      64           0 :     timeout_info.timeout = timeout;
+      65           0 :     timeout_info.callback = callback;
+      66           0 :     timeout_info.last_reset = last_reset;
+      67           0 :   }
+      68             : 
+      69           0 :   ros::Time TimeoutManager::lastReset(const timeout_id_t id)
+      70             :   {
+      71           0 :     std::scoped_lock lck(m_mtx);
+      72           0 :     return m_timeouts.at(id).last_reset;
+      73             :   }
+      74             : 
+      75        1562 :   bool TimeoutManager::started(const timeout_id_t id)
+      76             :   {
+      77        1562 :     std::scoped_lock lck(m_mtx);
+      78        3124 :     return m_timeouts.at(id).started;
+      79             :   }
+      80             : 
+      81       20135 :   void TimeoutManager::main_timer_callback([[maybe_unused]] const ros::TimerEvent &evt)
+      82             :   {
+      83       20135 :     const auto now = ros::Time::now();
+      84             : 
+      85       40270 :     std::scoped_lock lck(m_mtx);
+      86       46552 :     for (auto& timeout_info : m_timeouts)
+      87             :     {
+      88             :       // don't worry, this'll get optimized out by the compiler
+      89       26417 :       const bool started = timeout_info.started;
+      90       26417 :       const bool last_reset_timeout = now - timeout_info.last_reset >= timeout_info.timeout;
+      91       26417 :       const bool last_callback_timeout = now - timeout_info.last_callback >= timeout_info.timeout;
+      92       26417 :       if (started && last_reset_timeout && last_callback_timeout)
+      93             :       {
+      94        2058 :         timeout_info.callback(timeout_info.last_reset);
+      95        2058 :         timeout_info.last_callback = now;
+      96             :         // if the timeout is oneshot, pause it
+      97        2058 :         if (timeout_info.oneshot)
+      98           0 :           timeout_info.started = false;
+      99             :       }
+     100             :     }
+     101       20135 :   }
+     102             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..0b582c4d4a --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html @@ -0,0 +1,46 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c867c6a7540f7d57f11eda65b1d826deefd78012 GIT binary patch literal 512 zcmV+b0{{JqP)I*~y25zOg^hr1xiNM+LG`58W&G$N9I|OP#X`T26 zz@*c|b>>k1w4ja|9@(hw%rz(Ay>h5(ab;1n0$wqzKKOxp#J!%Up7I)M2LZn5^~iOmac@97IKq74SZ?s&eJvH{0Jg=d3&8 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
<unnamed>84.0 %105 / 12583.3 %15 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-detail-sort-l.html b/mrs_lib/src/timer/index-detail-sort-l.html new file mode 100644 index 0000000000..0338df18e3 --- /dev/null +++ b/mrs_lib/src/timer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
<unnamed>84.0 %105 / 12583.3 %15 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-detail.html b/mrs_lib/src/timer/index-detail.html new file mode 100644 index 0000000000..7895a16044 --- /dev/null +++ b/mrs_lib/src/timer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
<unnamed>84.0 %105 / 12583.3 %15 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-sort-f.html b/mrs_lib/src/timer/index-sort-f.html new file mode 100644 index 0000000000..61cd26f6ee --- /dev/null +++ b/mrs_lib/src/timer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-sort-l.html b/mrs_lib/src/timer/index-sort-l.html new file mode 100644 index 0000000000..eb393da185 --- /dev/null +++ b/mrs_lib/src/timer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index.html b/mrs_lib/src/timer/index.html new file mode 100644 index 0000000000..91fc4fdb6a --- /dev/null +++ b/mrs_lib/src/timer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/timer.cpp.func-sort-c.html b/mrs_lib/src/timer/timer.cpp.func-sort-c.html new file mode 100644 index 0000000000..6b062a9e41 --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ROSTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::threadFcn()8
mrs_lib::ThreadTimer::Impl::Impl(std::function<void (ros::TimerEvent const&)> const&, ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::~Impl()8
mrs_lib::ThreadTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ROSTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::stop()24
mrs_lib::ThreadTimer::Impl::start()24
mrs_lib::ThreadTimer::stop()24
mrs_lib::ThreadTimer::start()24
mrs_lib::ROSTimer::stop()24
mrs_lib::ROSTimer::start()24
mrs_lib::ThreadTimer::Impl::breakableSleep(ros::Time const&)208
mrs_lib::ThreadTimer::running()208
mrs_lib::ROSTimer::running()215
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/timer.cpp.func.html b/mrs_lib/src/timer/timer.cpp.func.html new file mode 100644 index 0000000000..26cbef6887 --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::breakableSleep(ros::Time const&)208
mrs_lib::ThreadTimer::Impl::stop()24
mrs_lib::ThreadTimer::Impl::start()24
mrs_lib::ThreadTimer::Impl::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::threadFcn()8
mrs_lib::ThreadTimer::Impl::Impl(std::function<void (ros::TimerEvent const&)> const&, ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::~Impl()8
mrs_lib::ThreadTimer::stop()24
mrs_lib::ThreadTimer::start()24
mrs_lib::ThreadTimer::running()208
mrs_lib::ThreadTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ROSTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ROSTimer::stop()24
mrs_lib::ROSTimer::start()24
mrs_lib::ROSTimer::running()215
mrs_lib::ROSTimer::setPeriod(ros::Duration const&, bool)8
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/timer.cpp.gcov.frameset.html b/mrs_lib/src/timer/timer.cpp.gcov.frameset.html new file mode 100644 index 0000000000..0c8c06a90b --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/timer/timer.cpp.gcov.html b/mrs_lib/src/timer/timer.cpp.gcov.html new file mode 100644 index 0000000000..2f2697dd59 --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.gcov.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-04-25 05:42:57Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/timer.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6             : // | ------------------------ ROSTimer ------------------------ |
+       7             : 
+       8             : /* stop() //{ */
+       9             : 
+      10          24 : void ROSTimer::stop() {
+      11             : 
+      12          48 :   std::scoped_lock lock(mutex_timer_);
+      13             : 
+      14          24 :   if (timer_) {
+      15          24 :     timer_->stop();
+      16             :   }
+      17          24 : }
+      18             : 
+      19             : //}
+      20             : 
+      21             : /* start() //{ */
+      22             : 
+      23          24 : void ROSTimer::start() {
+      24             : 
+      25          48 :   std::scoped_lock lock(mutex_timer_);
+      26             : 
+      27          24 :   if (timer_) {
+      28          24 :     timer_->start();
+      29             :   }
+      30          24 : }
+      31             : 
+      32             : //}
+      33             : 
+      34             : /* setPeriod() //{ */
+      35             : 
+      36           8 : void ROSTimer::setPeriod(const ros::Duration& duration, const bool reset) {
+      37             : 
+      38          16 :   std::scoped_lock lock(mutex_timer_);
+      39             : 
+      40           8 :   if (timer_) {
+      41           8 :     timer_->setPeriod(duration, reset);
+      42             :   }
+      43           8 : }
+      44             : 
+      45             : //}
+      46             : //
+      47             : /* setCallback() //{ */
+      48             : 
+      49           0 : void ROSTimer::setCallback([[maybe_unused]] const std::function<void(const ros::TimerEvent&)>& callback) {
+      50             : 
+      51           0 :   ROS_ERROR("[mrs_lib::ROSTimer]: setCallback(...) is not implemented for ROSTimer! Check [mrs_lib/src/timer/timer.cpp].");
+      52           0 : }
+      53             : 
+      54             : //}
+      55             : 
+      56             : /* running() //{ */
+      57             : 
+      58         215 : bool ROSTimer::running()
+      59             : {
+      60         430 :   std::scoped_lock lock(mutex_timer_);
+      61             : 
+      62         215 :   if (timer_)
+      63         215 :     return timer_->hasStarted();
+      64             :   else
+      65           0 :     return false;
+      66             : }
+      67             : 
+      68             : //}
+      69             : 
+      70             : // | ----------------------- ThreadTimer ---------------------- |
+      71             : 
+      72             : /* TheadTimer::start() //{ */
+      73             : 
+      74          24 : void ThreadTimer::start() {
+      75             : 
+      76          24 :   if (impl_) {
+      77          24 :     impl_->start();
+      78             :   }
+      79          24 : }
+      80             : 
+      81             : //}
+      82             : 
+      83             : /* ThreadTimer::stop() //{ */
+      84             : 
+      85          24 : void ThreadTimer::stop() {
+      86             : 
+      87          24 :   if (impl_) {
+      88          24 :     impl_->stop();
+      89             :   }
+      90          24 : }
+      91             : 
+      92             : //}
+      93             : 
+      94             : /* ThreadTimer::setPeriod() //{ */
+      95             : 
+      96           8 : void ThreadTimer::setPeriod(const ros::Duration& duration, [[maybe_unused]] const bool reset) {
+      97             : 
+      98           8 :   if (impl_) {
+      99           8 :     impl_->setPeriod(duration, reset);
+     100             :   }
+     101           8 : }
+     102             : 
+     103             : //}
+     104             : 
+     105             : /* ThreadTimer::setCallback() //{ */
+     106             : 
+     107           0 : void ThreadTimer::setCallback(const std::function<void(const ros::TimerEvent&)>& callback) {
+     108           0 :   if (impl_) {
+     109           0 :     impl_->setCallback(callback);
+     110             :   }
+     111           0 : }
+     112             : 
+     113             : //}
+     114             : 
+     115             : /* ThreadTimer::running() //{ */
+     116             : 
+     117         208 : bool ThreadTimer::running()
+     118             : {
+     119         208 :   if (impl_)
+     120         208 :     return impl_->running_;
+     121             :   else
+     122           0 :     return false;
+     123             : }
+     124             : 
+     125             : //}
+     126             : 
+     127             : /* ThreadTimer::Impl //{ */
+     128             : 
+     129           8 : ThreadTimer::Impl::Impl(const std::function<void(const ros::TimerEvent&)>& callback, const ros::Duration& delay_dur, const bool oneshot)
+     130           8 :   : callback_(callback), oneshot_(oneshot), delay_dur_(delay_dur)
+     131             : {
+     132           8 :   ending_  = false;
+     133           8 :   running_ = false;
+     134             : 
+     135           8 :   last_real_     = ros::Time(0);
+     136           8 :   last_expected_ = ros::Time(0);
+     137           8 :   next_expected_ = ros::Time(0);
+     138             : 
+     139           8 :   thread_ = std::thread(&ThreadTimer::Impl::threadFcn, this);
+     140           8 : }
+     141             : 
+     142           8 : ThreadTimer::Impl::~Impl()
+     143             : {
+     144             :   {
+     145             :     // signal the thread to end
+     146          16 :     std::scoped_lock lck(mutex_wakeup_, mutex_state_);
+     147           8 :     ending_ = true;
+     148           8 :     wakeup_cond_.notify_all();
+     149             :   }
+     150             :   // wait for it to die
+     151           8 :   thread_.join();
+     152           8 : }
+     153             : 
+     154          24 : void ThreadTimer::Impl::start()
+     155             : {
+     156          48 :   std::scoped_lock lck(mutex_wakeup_, mutex_state_);
+     157          24 :   if (!running_)
+     158             :   {
+     159          24 :     next_expected_ = ros::Time::now() + delay_dur_;
+     160          24 :     running_ = true;
+     161          24 :     wakeup_cond_.notify_all();
+     162             :   }
+     163          24 : }
+     164             : 
+     165          24 : void ThreadTimer::Impl::stop()
+     166             : {
+     167          48 :   std::scoped_lock lck(mutex_wakeup_, mutex_state_);
+     168          24 :   running_ = false;
+     169          24 :   wakeup_cond_.notify_all();
+     170          24 : }
+     171             : 
+     172           8 : void ThreadTimer::Impl::setPeriod(const ros::Duration& duration, [[maybe_unused]] const bool reset)
+     173             : {
+     174          16 :   std::scoped_lock lock(mutex_wakeup_, mutex_state_);
+     175           8 :   delay_dur_ = duration;
+     176             :   // gracefully handle the special case
+     177           8 :   if (duration == ros::Duration(0))
+     178           0 :     this->oneshot_  = true;
+     179           8 : }
+     180             : 
+     181           0 : void ThreadTimer::Impl::setCallback(const std::function<void(const ros::TimerEvent&)>& callback){
+     182           0 :   std::scoped_lock lock(mutex_state_);
+     183             : 
+     184           0 :   callback_ = callback;
+     185           0 : }
+     186             : 
+     187             : //}
+     188             : 
+     189             : /* ThreadTimer::breakableSleep() method //{ */
+     190         408 : bool ThreadTimer::Impl::breakableSleep(const ros::Time& until)
+     191             : {
+     192         408 :   while (ros::ok() && ros::Time::now() < until)
+     193             :   {
+     194         208 :     const std::chrono::nanoseconds dur {(until - ros::Time::now()).toNSec()};
+     195         208 :     const std::chrono::time_point<std::chrono::steady_clock> until_stl = std::chrono::steady_clock::now() + dur;
+     196         208 :     std::unique_lock lck(mutex_wakeup_);
+     197         208 :     if (!ending_ && running_)
+     198         208 :       wakeup_cond_.wait_until(lck, until_stl);
+     199             :     // check the flags while mutex_state_ is locked
+     200         208 :     if (ending_ || !running_)
+     201           8 :       return false;
+     202             :   }
+     203         200 :   return true;
+     204             : }
+     205             : //}
+     206             : 
+     207             : /* ThreadTimer::Impl::threadFcn() //{ */
+     208             : 
+     209         216 : void ThreadTimer::Impl::threadFcn()
+     210             : {
+     211         216 :   while (ros::ok() && !ending_)
+     212             :   {
+     213             :     {
+     214         216 :       std::unique_lock lck(mutex_wakeup_);
+     215             :       // if the timer is not yet started, wait for condition variable notification
+     216         216 :       if (!running_ && !ending_)
+     217          16 :         wakeup_cond_.wait(lck);
+     218             :       // The thread either got cv-notified, or running_ was already true, or it got woken up for some other reason.
+     219             :       // Check if the reason is that we should end (and end if it is).
+     220         216 :       if (ending_)
+     221           8 :         break;
+     222             :       // Check if the timer is still paused - probably was a spurious wake up (and restart the wait if it is).
+     223         208 :       if (!running_)
+     224           0 :         continue;
+     225             :     }
+     226             : 
+     227             :     // If the flow got here, the thread should attempt to wait for the specified duration.
+     228             :     // first, copy the delay we should wait for in a thread-safe manner
+     229         208 :     ros::Time next_expected;
+     230             :     {
+     231         208 :       std::scoped_lock lck(mutex_state_);
+     232         208 :       next_expected = next_expected_;
+     233             :     }
+     234         208 :     const bool interrupted = !breakableSleep(next_expected);
+     235         208 :     if (interrupted)
+     236           8 :       continue;
+     237             : 
+     238             :     {
+     239             :       // make sure the state doesn't change here
+     240         200 :       std::scoped_lock lck(mutex_state_);
+     241             :       // Again, check if everything is OK (the state may have changed while the thread was a sleeping beauty).
+     242             :       // Check if we should end (and end if it is so).
+     243         200 :       if (ending_)
+     244           0 :         break;
+     245             :       // Check if the timer is paused (and skip if it is so).
+     246         200 :       if (!running_)
+     247           0 :         continue;
+     248             : 
+     249             :       // If all is fine and dandy, actually run the callback function!
+     250             :       // if the timer is a oneshot-type, automatically pause it
+     251             :       // and do not fill out the expected fields in timer_event (we had no expectations...)
+     252         200 :       const ros::Time now = ros::Time::now();
+     253             :       // prepare the timer event
+     254         200 :       ros::TimerEvent timer_event;
+     255         200 :       if (oneshot_)
+     256             :       {
+     257           0 :         running_ = false;
+     258           0 :         timer_event.last_real        = last_real_;
+     259           0 :         timer_event.current_real     = now;
+     260             :       }
+     261             :       else
+     262             :       {
+     263         200 :         timer_event.last_expected    = last_expected_;
+     264         200 :         timer_event.last_real        = last_real_;
+     265         200 :         timer_event.current_expected = next_expected_;
+     266         200 :         timer_event.current_real     = now;
+     267             : 
+     268         200 :         last_expected_ = next_expected_;
+     269         200 :         next_expected_ = now + delay_dur_;
+     270             :       }
+     271         200 :       last_real_ = now;
+     272             :       // call the callback
+     273         200 :       callback_(timer_event);
+     274             :     }
+     275             :   }
+     276           8 : }
+     277             : 
+     278             : //}
+     279             : 
+     280             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/timer.cpp.gcov.overview.html b/mrs_lib/src/timer/timer.cpp.gcov.overview.html new file mode 100644 index 0000000000..68ddfa09da --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.gcov.overview.html @@ -0,0 +1,90 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/timer/timer.cpp.gcov.png b/mrs_lib/src/timer/timer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..02902cc75b73433a383fcc0da608c79be7330961 GIT binary patch literal 1150 zcmV-^1cCdBP)gy$&H153T1yMq zl!ZVcu46InK!{6l+OOygpe-Dsfm)On2c3)p8daVQH3`@dwNkuja8`cWXQUjt4@aK?&(Bdv&G0bwx@znnfS<x<$a|&p_j@;g_@!{2j+`%AL1AQn`9OkB~5w4Ho_Tgxloj-;0aOMcC=(<(EtIn1T^QmHcu!A5q$!L!{D)pa&iGDK zZAz}#x2;I3UEmt85T$Fl&FEfga`)=Fy?fY=EOF#5th(7Tqd7KUevXla;MOhJMv<{} zWS$l+v=zVEWT1V>I!#ZEqYYPQPUs47pJc_WE~^&WN+A$H&eZ91%09rGhj>cSJRy2?H6x<-(J=Tf%vbN3vvEZ0 zf#GQXJh>f10<`a+CnwqhMYY*^MoLR$iM{Vnrm6i&WOjHJ1FcCF8}OOLsP@%?P9o#0 zj)Te|k=Mxs`GpWr7Cj`8UqS*@NJy$Dn!AklbTlM!t^b7EfG?;<&F>fg1GM;S?E+(5 Q?EnA(07*qoM6N<$f;*}mH2?qr literal 0 HcmV?d00001 diff --git a/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html b/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html new file mode 100644 index 0000000000..a3b178e45f --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_broadcaster.cpp +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
<unnamed>53.3 %16 / 3066.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/index-detail-sort-l.html b/mrs_lib/src/transform_broadcaster/index-detail-sort-l.html new file mode 100644 index 0000000000..501ab1ae5a --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_broadcaster.cpp +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
<unnamed>53.3 %16 / 3066.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/index-detail.html b/mrs_lib/src/transform_broadcaster/index-detail.html new file mode 100644 index 0000000000..dd2817b002 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_broadcaster.cpp +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
<unnamed>53.3 %16 / 3066.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/index-sort-f.html b/mrs_lib/src/transform_broadcaster/index-sort-f.html new file mode 100644 index 0000000000..bd6c3232dc --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_broadcaster.cpp +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/index-sort-l.html b/mrs_lib/src/transform_broadcaster/index-sort-l.html new file mode 100644 index 0000000000..69456bfcb6 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_broadcaster.cpp +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/index.html b/mrs_lib/src/transform_broadcaster/index.html new file mode 100644 index 0000000000..56eda26e83 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_broadcaster.cpp +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html new file mode 100644 index 0000000000..d6b057e98f --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::TransformBroadcaster::sendTransform(std::vector<geometry_msgs::TransformStamped_<std::allocator<void> >, std::allocator<geometry_msgs::TransformStamped_<std::allocator<void> > > > const&)0
mrs_lib::TransformBroadcaster::TransformBroadcaster()179
mrs_lib::TransformBroadcaster::sendTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1839353
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html new file mode 100644 index 0000000000..5dcdb14682 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::TransformBroadcaster::sendTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1839353
mrs_lib::TransformBroadcaster::sendTransform(std::vector<geometry_msgs::TransformStamped_<std::allocator<void> >, std::allocator<geometry_msgs::TransformStamped_<std::allocator<void> > > > const&)0
mrs_lib::TransformBroadcaster::TransformBroadcaster()179
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.frameset.html b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.frameset.html new file mode 100644 index 0000000000..b73151ec52 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html new file mode 100644 index 0000000000..b5800ed031 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/transform_broadcaster.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6             : /* constructor //{ */
+       7         179 : TransformBroadcaster::TransformBroadcaster() {
+       8         179 :   broadcaster_ = tf2_ros::TransformBroadcaster();
+       9         179 : }
+      10             : //}
+      11             : 
+      12             : /* sendTransform (const geometry_msgs::TransformStamped &transform) //{ */
+      13     1839353 : void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) {
+      14     3676903 :   std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      15     1836101 :   if (last_messages_.count(frames_from_to) > 0) {
+      16     1837673 :     if (transform.header.stamp > last_messages_[frames_from_to]) {
+      17     1550547 :       broadcaster_.sendTransform(transform);
+      18     1553004 :       last_messages_[frames_from_to] = transform.header.stamp;
+      19             :     } else {
+      20      283740 :       ROS_WARN_ONCE("[%s]: TF_REPEATED_DATA ignoring data with redundant timestamp. Transform from frame '%s' to frame '%s'", ros::this_node::getName().c_str(),
+      21             :                     transform.header.frame_id.c_str(), transform.child_frame_id.c_str());
+      22             :     }
+      23             :   } else {
+      24        1842 :     std::pair<std::string, ros::Time> new_pair;
+      25         924 :     new_pair.first  = frames_from_to;
+      26         924 :     new_pair.second = transform.header.stamp;
+      27         924 :     broadcaster_.sendTransform(transform);
+      28         924 :     last_messages_.insert(new_pair);
+      29             :   }
+      30     1839518 : }
+      31             : //}
+      32             : 
+      33             : /* sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms) //{ */
+      34           0 : void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms) {
+      35           0 :   for (auto transform : transforms) {
+      36           0 :     std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      37           0 :     if (last_messages_.count(frames_from_to) > 0) {
+      38           0 :       if (transform.header.stamp > last_messages_[frames_from_to]) {
+      39           0 :         broadcaster_.sendTransform(transform);
+      40           0 :         last_messages_[frames_from_to] = transform.header.stamp;
+      41             :       } else {
+      42           0 :         ROS_WARN_ONCE("[%s]: TF_REPEATED_DATA ignoring data with redundant timestamp. Transform from frame '%s' to frame '%s'",
+      43             :                       ros::this_node::getName().c_str(), transform.header.frame_id.c_str(), transform.child_frame_id.c_str());
+      44             :       }
+      45             :     } else {
+      46           0 :       std::pair<std::string, ros::Time> new_pair;
+      47           0 :       new_pair.first  = frames_from_to;
+      48           0 :       new_pair.second = transform.header.stamp;
+      49           0 :       broadcaster_.sendTransform(transform);
+      50           0 :       last_messages_.insert(new_pair);
+      51             :     }
+      52             :   }
+      53           0 : }
+      54             : //}
+      55             : 
+      56             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html new file mode 100644 index 0000000000..f1fb284078 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html @@ -0,0 +1,34 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ef243e6e5fb1d3bcaec6f7b6354ea8ce8ebffdea GIT binary patch literal 425 zcmV;a0apHrP)M01uJ*nY`JnFx*N2@5{2?`a1q6zmssfL3_>!}8X35VVlYkpKa402t<;> + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transformer.cpp +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
<unnamed>66.1 %185 / 28076.9 %20 / 26
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/index-detail-sort-l.html b/mrs_lib/src/transformer/index-detail-sort-l.html new file mode 100644 index 0000000000..3f2c5348ea --- /dev/null +++ b/mrs_lib/src/transformer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transformer.cpp +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
<unnamed>66.1 %185 / 28076.9 %20 / 26
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/index-detail.html b/mrs_lib/src/transformer/index-detail.html new file mode 100644 index 0000000000..03f66f08b4 --- /dev/null +++ b/mrs_lib/src/transformer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transformer.cpp +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
<unnamed>66.1 %185 / 28076.9 %20 / 26
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/index-sort-f.html b/mrs_lib/src/transformer/index-sort-f.html new file mode 100644 index 0000000000..ce12457951 --- /dev/null +++ b/mrs_lib/src/transformer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transformer.cpp +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/index-sort-l.html b/mrs_lib/src/transformer/index-sort-l.html new file mode 100644 index 0000000000..fb80c28fa7 --- /dev/null +++ b/mrs_lib/src/transformer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transformer.cpp +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/index.html b/mrs_lib/src/transformer/index.html new file mode 100644 index 0000000000..70757bca68 --- /dev/null +++ b/mrs_lib/src/transformer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transformer.cpp +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/transformer.cpp.func-sort-c.html b/mrs_lib/src/transformer/transformer.cpp.func-sort-c.html new file mode 100644 index 0000000000..226424d3a1 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.func-sort-c.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Transformer::LLtoUTM(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::LLtoUTM(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::LLtoUTM(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::Transformer()0
mrs_lib::Transformer::operator=(mrs_lib::Transformer&&)0
void tf2::doTransform<cv::Point3_<double> >(cv::Point3_<double> const&, cv::Point3_<double>&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
mrs_lib::Transformer::getTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::Transformer::getTransformImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::Transformer::LLtoUTM(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)6
mrs_lib::Transformer::transformAsPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
mrs_lib::Transformer::transformAsPoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)6
mrs_lib::Transformer::transformAsVector(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
mrs_lib::Transformer::transformAsVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)6
mrs_lib::Transformer::Transformer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)12
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)653
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2004
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2004
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2006
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4018
mrs_lib::Transformer::getTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)22309
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)112766
mrs_lib::Transformer::setLatLon(double, double)142706
mrs_lib::Transformer::getTransformImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1601979
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6752864
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/transformer.cpp.func.html b/mrs_lib/src/transformer/transformer.cpp.func.html new file mode 100644 index 0000000000..e27b6505b4 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.func.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
void tf2::doTransform<cv::Point3_<double> >(cv::Point3_<double> const&, cv::Point3_<double>&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
mrs_lib::Transformer::getTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::Transformer::getTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)22309
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)6
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)112766
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4018
mrs_lib::Transformer::getTransformImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::Transformer::getTransformImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1601979
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6752864
mrs_lib::Transformer::transformAsPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
mrs_lib::Transformer::transformAsPoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)6
mrs_lib::Transformer::transformAsVector(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
mrs_lib::Transformer::transformAsVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)6
mrs_lib::Transformer::LLtoUTM(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::LLtoUTM(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::LLtoUTM(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::LLtoUTM(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2004
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PointStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2004
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2006
mrs_lib::Transformer::setLatLon(double, double)142706
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)653
mrs_lib::Transformer::Transformer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)12
mrs_lib::Transformer::Transformer()0
mrs_lib::Transformer::operator=(mrs_lib::Transformer&&)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/transformer.cpp.gcov.frameset.html b/mrs_lib/src/transformer/transformer.cpp.gcov.frameset.html new file mode 100644 index 0000000000..c4e71e8234 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/transformer/transformer.cpp.gcov.html b/mrs_lib/src/transformer/transformer.cpp.gcov.html new file mode 100644 index 0000000000..4062264e3a --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.gcov.html @@ -0,0 +1,671 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-25 05:42:57Functions:202676.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : 
+       3             : #include <mrs_lib/transformer.h>
+       4             : #include <mrs_lib/gps_conversions.h>
+       5             : #include <opencv2/core/types.hpp>
+       6             : #include <mrs_lib/geometry/conversions.h>
+       7             : #include <mutex>
+       8             : 
+       9             : using test_t = mrs_msgs::ReferenceStamped;
+      10             : /* using test_t = pcl::PointCloud<pcl::PointXYZ>; */
+      11             : template std::optional<test_t> mrs_lib::Transformer::transform<test_t>(const test_t& what, const geometry_msgs::TransformStamped& to_frame);
+      12             : template std::optional<test_t::Ptr> mrs_lib::Transformer::transform<test_t>(const test_t::Ptr& what, const geometry_msgs::TransformStamped& to_frame);
+      13             : template std::optional<test_t::Ptr> mrs_lib::Transformer::transform<test_t>(const test_t::ConstPtr& what, const geometry_msgs::TransformStamped& to_frame);
+      14             : template std::optional<test_t> mrs_lib::Transformer::transformSingle<test_t>(const test_t& what, const std::string& to_frame);
+      15             : template std::optional<test_t::Ptr> mrs_lib::Transformer::transformSingle<test_t>(const test_t::Ptr& what, const std::string& to_frame);
+      16             : template std::optional<test_t::Ptr> mrs_lib::Transformer::transformSingle<test_t>(const test_t::ConstPtr& what, const std::string& to_frame);
+      17             : 
+      18             : namespace tf2
+      19             : {
+      20             :   template <>
+      21           1 :   void doTransform(const cv::Point3d& point_in, cv::Point3d& point_out, const geometry_msgs::TransformStamped& transform)
+      22             :   {
+      23           1 :     const geometry_msgs::Point pt = mrs_lib::geometry::fromCV(point_in);
+      24           1 :     geometry_msgs::Point pt_tfd;
+      25           1 :     tf2::doTransform(pt, pt_tfd, transform);
+      26           1 :     point_out = mrs_lib::geometry::toCV(pt_tfd);
+      27           1 :   }
+      28             : }
+      29             : 
+      30             : namespace mrs_lib
+      31             : {
+      32             : 
+      33             :   // | ----------------------- Transformer ---------------------- |
+      34             : 
+      35             :   // | ------------------ user-callable methods ----------------- |
+      36             : 
+      37             :   /* Transformer() (constructors)//{ */
+      38             : 
+      39           0 :   Transformer::Transformer()
+      40             :   {
+      41           0 :   }
+      42             : 
+      43          12 :   Transformer::Transformer(const std::string& node_name, const ros::Duration& cache_time)
+      44          12 :     : initialized_(true), node_name_(node_name), tf_buffer_(std::make_unique<tf2_ros::Buffer>(cache_time)), tf_listener_ptr_(std::make_unique<tf2_ros::TransformListener>(*tf_buffer_))
+      45             :   {
+      46          12 :   }
+      47             : 
+      48         653 :   Transformer::Transformer(const ros::NodeHandle& nh, const std::string& node_name, const ros::Duration& cache_time)
+      49         653 :     : initialized_(true), node_name_(node_name), tf_buffer_(std::make_unique<tf2_ros::Buffer>(cache_time)), tf_listener_ptr_(std::make_unique<tf2_ros::TransformListener>(*tf_buffer_, nh))
+      50             :   {
+      51         653 :   }
+      52             : 
+      53           0 :   Transformer& Transformer::operator=(Transformer&& other)
+      54             :   {
+      55           0 :     std::scoped_lock lck(other.mutex_, mutex_);
+      56             : 
+      57           0 :     initialized_ = std::move(other.initialized_);
+      58           0 :     node_name_ = std::move(other.node_name_);
+      59           0 :     tf_buffer_ = std::move(other.tf_buffer_);
+      60           0 :     tf_listener_ptr_ = std::move(other.tf_listener_ptr_);
+      61             : 
+      62           0 :     default_frame_id_ = std::move(other.default_frame_id_);
+      63           0 :     prefix_ = std::move(other.prefix_);
+      64           0 :     quiet_ = std::move(other.quiet_);
+      65           0 :     lookup_timeout_ = std::move(other.lookup_timeout_);
+      66           0 :     retry_lookup_newest_ = std::move(other.retry_lookup_newest_);
+      67             : 
+      68           0 :     got_utm_zone_ = std::move(other.got_utm_zone_);
+      69           0 :     utm_zone_ = std::move(other.utm_zone_);
+      70             : 
+      71           0 :     return *this;
+      72             :   }
+      73             : 
+      74             :   //}
+      75             : 
+      76             :   /* getTransform() //{ */
+      77             : 
+      78       22309 :   std::optional<geometry_msgs::TransformStamped> Transformer::getTransform(const std::string& from_frame_raw, const std::string& to_frame_raw, const ros::Time& time_stamp)
+      79             :   {
+      80       44621 :     std::scoped_lock lck(mutex_);
+      81             : 
+      82       22312 :     if (!initialized_)
+      83             :     {
+      84           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot provide transform, not initialized!", node_name_.c_str());
+      85           0 :       return std::nullopt;
+      86             :     }
+      87             : 
+      88             :     // resolve the frames
+      89       44624 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+      90       44624 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+      91       44624 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+      92             : 
+      93       22312 :     return getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+      94             :   }
+      95             : 
+      96           1 :   std::optional<geometry_msgs::TransformStamped> Transformer::getTransform(const std::string& from_frame_raw, const ros::Time& from_stamp, const std::string& to_frame_raw, const ros::Time& to_stamp, const std::string& fixed_frame_raw)
+      97             :   {
+      98           2 :     std::scoped_lock lck(mutex_);
+      99             : 
+     100           1 :     if (!initialized_)
+     101             :     {
+     102           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot provide transform, not initialized", node_name_.c_str());
+     103           0 :       return std::nullopt;
+     104             :     }
+     105             : 
+     106           2 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     107           2 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     108           2 :     const std::string fixed_frame = resolveFrameImpl(fixed_frame_raw);
+     109           2 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     110             : 
+     111           1 :     return getTransformImpl(from_frame, from_stamp, to_frame, to_stamp, fixed_frame, latlon_frame);
+     112             :   }
+     113             :   //}
+     114             : 
+     115             :   /* setLatLon() //{ */
+     116             : 
+     117      142706 :   void Transformer::setLatLon(const double lat, const double lon)
+     118             :   {
+     119      142706 :     std::scoped_lock lck(mutex_);
+     120             : 
+     121             :     double utm_x, utm_y;
+     122      142187 :     mrs_lib::LLtoUTM(lat, lon, utm_y, utm_x, utm_zone_.data());
+     123      142183 :     got_utm_zone_ = true;
+     124      141557 :   }
+     125             : 
+     126             :   //}
+     127             : 
+     128             :   /* transformAsVector() //{ */
+     129             : 
+     130           6 :   [[nodiscard]] std::optional<Eigen::Vector3d> Transformer::transformAsVector(const Eigen::Vector3d& what, const geometry_msgs::TransformStamped& tf)
+     131             :   {
+     132          12 :     std::scoped_lock lck(mutex_);
+     133             : 
+     134           6 :     if (!initialized_)
+     135             :     {
+     136           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform, not initialized", node_name_.c_str());
+     137           0 :       return std::nullopt;
+     138             :     }
+     139             : 
+     140          12 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     141          12 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     142          12 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     143             : 
+     144           6 :     const geometry_msgs::Vector3 vec = mrs_lib::geometry::fromEigenVec(what);
+     145           6 :     const auto tfd_vec = transformImpl(tf_resolved, vec);
+     146           6 :     if (tfd_vec.has_value())
+     147          12 :       return mrs_lib::geometry::toEigen(tfd_vec.value());
+     148             :     else
+     149           0 :       return std::nullopt;
+     150             :   }
+     151             : 
+     152           6 :   [[nodiscard]] std::optional<Eigen::Vector3d> Transformer::transformAsVector(const std::string& from_frame_raw, const Eigen::Vector3d& what, const std::string& to_frame_raw, const ros::Time& time_stamp)
+     153             :   {
+     154          12 :     std::scoped_lock lck(mutex_);
+     155             : 
+     156           6 :     if (!initialized_)
+     157             :     {
+     158           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform, not initialized", node_name_.c_str());
+     159           0 :       return std::nullopt;
+     160             :     }
+     161             : 
+     162          12 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     163          12 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     164          12 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     165             : 
+     166             :     // get the transform
+     167          12 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     168           6 :     if (!tf_opt.has_value())
+     169           0 :       return std::nullopt;
+     170           6 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     171             : 
+     172             :     // do the transformation
+     173          12 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     174           6 :     const geometry_msgs::Vector3 vec = mrs_lib::geometry::fromEigenVec(what);
+     175           6 :     const auto tfd_vec = transformImpl(tf_resolved, vec);
+     176           6 :     if (tfd_vec.has_value())
+     177          12 :       return mrs_lib::geometry::toEigen(tfd_vec.value());
+     178             :     else
+     179           0 :       return std::nullopt;
+     180             :   }
+     181             : 
+     182             :   /* //} */
+     183             : 
+     184             :   /* transformAsPoint() //{ */
+     185             : 
+     186           6 :   [[nodiscard]] std::optional<Eigen::Vector3d> Transformer::transformAsPoint(const Eigen::Vector3d& what, const geometry_msgs::TransformStamped& tf)
+     187             :   {
+     188          12 :     std::scoped_lock lck(mutex_);
+     189             : 
+     190           6 :     if (!initialized_)
+     191             :     {
+     192           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform, not initialized", node_name_.c_str());
+     193           0 :       return std::nullopt;
+     194             :     }
+     195             : 
+     196          12 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     197          12 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     198          12 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     199             : 
+     200           6 :     geometry_msgs::Point pt;
+     201           6 :     pt.x = what.x();
+     202           6 :     pt.y = what.y();
+     203           6 :     pt.z = what.z();
+     204           6 :     const auto tfd_pt = transformImpl(tf_resolved, pt);
+     205           6 :     if (tfd_pt.has_value())
+     206          12 :       return mrs_lib::geometry::toEigen(tfd_pt.value());
+     207             :     else
+     208           0 :       return std::nullopt;
+     209             :   }
+     210             : 
+     211           6 :   [[nodiscard]] std::optional<Eigen::Vector3d> Transformer::transformAsPoint(const std::string& from_frame_raw, const Eigen::Vector3d& what, const std::string& to_frame_raw, const ros::Time& time_stamp)
+     212             :   {
+     213          12 :     std::scoped_lock lck(mutex_);
+     214             : 
+     215           6 :     if (!initialized_)
+     216             :     {
+     217           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform, not initialized", node_name_.c_str());
+     218           0 :       return std::nullopt;
+     219             :     }
+     220             : 
+     221          12 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     222          12 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     223          12 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     224             : 
+     225             :     // get the transform
+     226          12 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     227           6 :     if (!tf_opt.has_value())
+     228           0 :       return std::nullopt;
+     229           6 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     230             : 
+     231             :     // do the transformation
+     232          12 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     233           6 :     geometry_msgs::Point pt;
+     234           6 :     pt.x = what.x();
+     235           6 :     pt.y = what.y();
+     236           6 :     pt.z = what.z();
+     237           6 :     const auto tfd_pt = transformImpl(tf_resolved, pt);
+     238           6 :     if (tfd_pt.has_value())
+     239          12 :       return mrs_lib::geometry::toEigen(tfd_pt.value());
+     240             :     else
+     241           0 :       return std::nullopt;
+     242             :   }
+     243             : 
+     244             :   /* //} */
+     245             : 
+     246             :   // | ------------- helper implementation methods -------------- |
+     247             : 
+     248             :   /* transformImpl() //{ */
+     249             : 
+     250             :   /* specialization for mrs_msgs::ReferenceStamped //{ */
+     251             :   
+     252      112766 :   std::optional<mrs_msgs::ReferenceStamped> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what)
+     253             :   {
+     254             :     // create a pose message
+     255      225532 :     geometry_msgs::PoseStamped pose;
+     256      112766 :     pose.header = what.header;
+     257             :   
+     258      112766 :     pose.pose.position.x = what.reference.position.x;
+     259      112766 :     pose.pose.position.y = what.reference.position.y;
+     260      112766 :     pose.pose.position.z = what.reference.position.z;
+     261      112766 :     pose.pose.orientation = geometry::fromEigen(geometry::quaternionFromHeading(what.reference.heading));
+     262             :   
+     263             :     // try to transform the pose message
+     264      225531 :     const auto pose_opt = transformImpl(tf, pose);
+     265      112766 :     if (!pose_opt.has_value())
+     266           0 :       return std::nullopt;
+     267             :     // overwrite the pose with it's transformed value
+     268      112766 :     pose = pose_opt.value();
+     269             :   
+     270      225531 :     mrs_msgs::ReferenceStamped ret;
+     271      112766 :     ret.header = pose.header;
+     272             :   
+     273             :     // copy the new transformed data back
+     274      112766 :     ret.reference.position.x = pose.pose.position.x;
+     275      112766 :     ret.reference.position.y = pose.pose.position.y;
+     276      112766 :     ret.reference.position.z = pose.pose.position.z;
+     277      112766 :     ret.reference.heading = geometry::headingFromRot(geometry::toEigen(pose.pose.orientation));
+     278      112766 :     return ret;
+     279             :   }
+     280             :   
+     281             :   //}
+     282             : 
+     283             :   /* specialization for Eigen::Vector3d //{ */
+     284             :   
+     285           6 :   std::optional<Eigen::Vector3d> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const Eigen::Vector3d& what)
+     286             :   {
+     287             :     // just transform it as you would a geometry_msgs::Vector3
+     288           6 :     const geometry_msgs::Vector3 as_vec = mrs_lib::geometry::fromEigenVec(what);
+     289           6 :     const auto opt = transformImpl(tf, as_vec);
+     290           6 :     if (opt.has_value())
+     291          12 :       return geometry::toEigen(opt.value());
+     292             :     else
+     293           0 :       return std::nullopt;
+     294             :   }
+     295             :   
+     296             :   //}
+     297             : 
+     298             :   //}
+     299             : 
+     300             :   /* getTransformImpl() //{ */
+     301             : 
+     302     1601979 :   std::optional<geometry_msgs::TransformStamped> Transformer::getTransformImpl(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp, const std::string& latlon_frame)
+     303             :   {
+     304     1601979 :     if (!initialized_)
+     305             :     {
+     306           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot provide transform, not initialized!", node_name_.c_str());
+     307           0 :       return std::nullopt;
+     308             :     }
+     309             : 
+     310             :     // if any of the frames is empty, then the query is invalid, return nullopt
+     311     1601979 :     if (from_frame.empty() || to_frame.empty())
+     312             :     {
+     313           3 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform to/from an empty frame!", node_name_.c_str());
+     314           3 :       return std::nullopt;
+     315             :     }
+     316             : 
+     317             :     // if the frames are the same, just return an identity transform
+     318     1601969 :     if (from_frame == to_frame)
+     319      358397 :       return create_transform(from_frame, to_frame, time_stamp, tf2::toMsg(tf2::Transform::getIdentity()));
+     320             : 
+     321             :     // check for a transform from/to latlon coordinates - that is a special case handled separately
+     322     1422778 :     if (from_frame == latlon_frame)
+     323             :     {
+     324             :       // find the transformation between the UTM frame and the non-latlon frame to fill the returned tf
+     325           6 :       const std::string utm_frame = getFramePrefix(from_frame) + "utm_origin";
+     326           6 :       auto tf_opt = getTransformImpl(utm_frame, to_frame, time_stamp, latlon_frame);
+     327           3 :       if (!tf_opt.has_value())
+     328           0 :         return std::nullopt;
+     329             :       // change the transformation frames to point from latlon
+     330           3 :       frame_from(*tf_opt) = from_frame;
+     331           3 :       return tf_opt;
+     332             :     }
+     333     1422776 :     else if (to_frame == latlon_frame)
+     334             :     {
+     335             :       // find the transformation between the UTM frame and the non-latlon frame to fill the returned tf
+     336        4014 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     337        4014 :       auto tf_opt = getTransformImpl(from_frame, utm_frame, time_stamp, latlon_frame);
+     338        2007 :       if (!tf_opt.has_value())
+     339           0 :         return std::nullopt;
+     340             :       // change the transformation frames to point to latlon
+     341        2007 :       frame_to(*tf_opt) = to_frame;
+     342        2007 :       return tf_opt;
+     343             :     }
+     344             : 
+     345     4262260 :     tf2::TransformException ex("");
+     346             :     // first try to get transform at the requested time
+     347             :     try
+     348             :     {
+     349             :       // try looking up and returning the transform
+     350     1957581 :       return tf_buffer_->lookupTransform(to_frame, from_frame, time_stamp, lookup_timeout_);
+     351             :     }
+     352     1767857 :     catch (tf2::TransformException& e)
+     353             :     {
+     354      883933 :       ex = e;
+     355             :     }
+     356             : 
+     357             :     // if that failed, try to get the newest one if requested
+     358      883926 :     if (retry_lookup_newest_)
+     359             :     {
+     360             :       try
+     361             :       {
+     362     1755203 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     363             :       }
+     364       25118 :       catch (tf2::TransformException& e)
+     365             :       {
+     366       12559 :         ex = e;
+     367             :       }
+     368             :     }
+     369             : 
+     370             :     // if the flow got here, we've failed to look the transform up
+     371       12590 :     if (quiet_)
+     372             :     {
+     373           0 :       ROS_DEBUG("[%s]: Transformer: Exception caught while looking up transform from \"%s\" to \"%s\": %s", node_name_.c_str(), from_frame.c_str(),
+     374             :                 to_frame.c_str(), ex.what());
+     375             :     } else
+     376             :     {
+     377       12590 :       ROS_WARN_THROTTLE(1.0, "[%s]: Transformer: Exception caught while looking up transform from \"%s\" to \"%s\": %s", node_name_.c_str(),
+     378             :                         from_frame.c_str(), to_frame.c_str(), ex.what());
+     379             :     }
+     380             : 
+     381       12590 :     return std::nullopt;
+     382             :   }
+     383             : 
+     384           1 :   std::optional<geometry_msgs::TransformStamped> Transformer::getTransformImpl(const std::string& from_frame, const ros::Time& from_stamp, const std::string& to_frame, const ros::Time& to_stamp, const std::string& fixed_frame, const std::string& latlon_frame)
+     385             :   {
+     386           1 :     if (!initialized_)
+     387             :     {
+     388           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot provide transform, not initialized", node_name_.c_str());
+     389           0 :       return std::nullopt;
+     390             :     }
+     391             : 
+     392             :     // if the frames are the same, just return an identity transform
+     393           1 :     if (from_frame == to_frame)
+     394           0 :       return create_transform(from_frame, to_frame, to_stamp, tf2::toMsg(tf2::Transform::getIdentity()));
+     395             : 
+     396             :     // check for a transform from/to latlon coordinates - that is a special case handled separately
+     397           1 :     if (from_frame == latlon_frame)
+     398             :     {
+     399             :       // find the transformation between the UTM frame and the non-latlon frame to fill the returned tf
+     400           0 :       const std::string utm_frame = getFramePrefix(from_frame) + "utm_origin";
+     401           0 :       auto tf_opt = getTransformImpl(utm_frame, from_stamp, to_frame, to_stamp, fixed_frame, latlon_frame);
+     402           0 :       if (!tf_opt.has_value())
+     403           0 :         return std::nullopt;
+     404             :       // change the transformation frames to point from latlon
+     405           0 :       frame_from(*tf_opt) = from_frame;
+     406           0 :       return tf_opt;
+     407             :     }
+     408           1 :     else if (to_frame == latlon_frame)
+     409             :     {
+     410             :       // find the transformation between the UTM frame and the non-latlon frame to fill the returned tf
+     411           0 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     412           0 :       auto tf_opt = getTransformImpl(from_frame, from_stamp, utm_frame, to_stamp, fixed_frame, latlon_frame);
+     413           0 :       if (!tf_opt.has_value())
+     414           0 :         return std::nullopt;
+     415             :       // change the transformation frames to point to latlon
+     416           0 :       frame_to(*tf_opt) = to_frame;
+     417           0 :       return tf_opt;
+     418             :     }
+     419             : 
+     420           3 :     tf2::TransformException ex("");
+     421             :     // first try to get transform at the requested time
+     422             :     try
+     423             :     {
+     424             :       // try looking up and returning the transform
+     425           2 :       return tf_buffer_->lookupTransform(to_frame, to_stamp, from_frame, from_stamp, fixed_frame, lookup_timeout_);
+     426             :     }
+     427           0 :     catch (tf2::TransformException& e)
+     428             :     {
+     429           0 :       ex = e;
+     430             :     }
+     431             : 
+     432             :     // if that failed, try to get the newest one if requested
+     433           0 :     if (retry_lookup_newest_)
+     434             :     {
+     435             :       try
+     436             :       {
+     437           0 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     438             :       }
+     439           0 :       catch (tf2::TransformException& e)
+     440             :       {
+     441           0 :         ex = e;
+     442             :       }
+     443             :     }
+     444             : 
+     445             :     // if the flow got here, we've failed to look the transform up
+     446           0 :     if (quiet_)
+     447             :     {
+     448           0 :       ROS_DEBUG("[%s]: Transformer: Exception caught while looking up transform from \"%s\" to \"%s\": %s", node_name_.c_str(), from_frame.c_str(),
+     449             :                 to_frame.c_str(), ex.what());
+     450             :     } else
+     451             :     {
+     452           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: Transformer: Exception caught while looking up transform from \"%s\" to \"%s\": %s", node_name_.c_str(),
+     453             :                         from_frame.c_str(), to_frame.c_str(), ex.what());
+     454             :     }
+     455             : 
+     456           0 :     return std::nullopt;
+     457             :   }
+     458             : 
+     459             :   //}
+     460             : 
+     461             :   /* resolveFrameImpl() //{*/
+     462             : 
+     463     6752864 :   std::string Transformer::resolveFrameImpl(const std::string& frame_id)
+     464             :   {
+     465             :     // if the frame is empty, return the default frame id
+     466     6752864 :     if (frame_id.empty())
+     467        6346 :       return default_frame_id_;
+     468             : 
+     469             :     // if there is no prefix set, just return the raw frame id
+     470     6746506 :     if (prefix_.empty())
+     471     4117845 :       return frame_id;
+     472             : 
+     473             :     // if there is a default prefix set and the frame does not start with it, prefix it
+     474     2628551 :     if (frame_id.substr(0, prefix_.length()) != prefix_)
+     475     1656658 :       return prefix_ + frame_id;
+     476             : 
+     477      972023 :     return frame_id;
+     478             :   }
+     479             : 
+     480             :   //}
+     481             : 
+     482             :   /* LLtoUTM() method //{ */
+     483           2 :   geometry_msgs::Point Transformer::LLtoUTM(const geometry_msgs::Point& what, [[maybe_unused]] const std::string& prefix)
+     484             :   {
+     485             :     // convert LAT-LON to UTM
+     486           2 :     geometry_msgs::Point utm;
+     487           2 :     mrs_lib::UTM(what.x, what.y, &utm.x, &utm.y);
+     488             :     // copy the height from the input
+     489           2 :     utm.z = what.z;
+     490           2 :     return utm;
+     491             :   }
+     492             : 
+     493           0 :   geometry_msgs::PointStamped Transformer::LLtoUTM(const geometry_msgs::PointStamped& what, [[maybe_unused]] const std::string& prefix)
+     494             :   {
+     495           0 :     geometry_msgs::PointStamped ret;
+     496           0 :     ret.header.frame_id = prefix + "utm_origin";
+     497           0 :     ret.header.stamp = what.header.stamp;
+     498           0 :     ret.point = LLtoUTM(what.point, prefix);
+     499           0 :     return ret;
+     500             :   }
+     501             : 
+     502           0 :   geometry_msgs::Pose Transformer::LLtoUTM(const geometry_msgs::Pose& what, const std::string& prefix)
+     503             :   {
+     504           0 :     geometry_msgs::Pose ret;
+     505           0 :     ret.position = LLtoUTM(what.position, prefix);
+     506           0 :     ret.orientation = what.orientation;
+     507           0 :     return ret;
+     508             :   }
+     509             : 
+     510           0 :   geometry_msgs::PoseStamped Transformer::LLtoUTM(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     511             :   {
+     512           0 :     geometry_msgs::PoseStamped ret;
+     513           0 :     ret.header.frame_id = prefix + "utm_origin";
+     514           0 :     ret.header.stamp = what.header.stamp;
+     515           0 :     ret.pose = LLtoUTM(what.pose, prefix);
+     516           0 :     return ret;
+     517             :   }
+     518             :   //}
+     519             : 
+     520             :   /* UTMtoLL() method //{ */
+     521        2006 :   std::optional<geometry_msgs::Point> Transformer::UTMtoLL(const geometry_msgs::Point& what, [[maybe_unused]] const std::string& prefix)
+     522             :   {
+     523             :     // if no UTM zone was specified by the user, we don't know which one to use...
+     524        2006 :     if (!got_utm_zone_)
+     525             :     {
+     526           1 :       ROS_WARN_THROTTLE(1.0, "[%s]: cannot transform to latlong, missing UTM zone (did you call setLatLon()?)", node_name_.c_str());
+     527           1 :       return std::nullopt;
+     528             :     }
+     529             :   
+     530             :     // now apply the nonlinear transformation from UTM to LAT-LON
+     531        2005 :     geometry_msgs::Point latlon;
+     532        2005 :     mrs_lib::UTMtoLL(what.y, what.x, utm_zone_.data(), latlon.x, latlon.y);
+     533        2005 :     latlon.z = what.z;
+     534        2005 :     return latlon;
+     535             :   }
+     536             : 
+     537           0 :   std::optional<geometry_msgs::PointStamped> Transformer::UTMtoLL(const geometry_msgs::PointStamped& what, [[maybe_unused]] const std::string& prefix)
+     538             :   {
+     539           0 :     const auto opt = UTMtoLL(what.point, prefix);
+     540           0 :     if (!opt.has_value())
+     541           0 :       return std::nullopt;
+     542             : 
+     543           0 :     geometry_msgs::PointStamped ret;
+     544           0 :     ret.header.frame_id = prefix + "utm_origin";
+     545           0 :     ret.header.stamp = what.header.stamp;
+     546           0 :     ret.point = opt.value();
+     547           0 :     return ret;
+     548             :   }
+     549             : 
+     550        2004 :   std::optional<geometry_msgs::Pose> Transformer::UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix)
+     551             :   {
+     552        2004 :     const auto opt = UTMtoLL(what.position, prefix);
+     553        2004 :     if (!opt.has_value())
+     554           0 :       return std::nullopt;
+     555             : 
+     556        2004 :     geometry_msgs::Pose ret;
+     557        2004 :     ret.position = opt.value();
+     558        2004 :     ret.orientation = what.orientation;
+     559        2004 :     return ret;
+     560             :   }
+     561             : 
+     562        2004 :   std::optional<geometry_msgs::PoseStamped> Transformer::UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     563             :   {
+     564        2004 :     const auto opt = UTMtoLL(what.pose, prefix);
+     565        2004 :     if (!opt.has_value())
+     566           0 :       return std::nullopt;
+     567             : 
+     568        4008 :     geometry_msgs::PoseStamped ret;
+     569        2004 :     ret.header.frame_id = prefix + "utm_origin";
+     570        2004 :     ret.header.stamp = what.header.stamp;
+     571        2004 :     ret.pose = opt.value();
+     572        2004 :     return ret;
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* getFramePrefix() method //{ */
+     577        4018 :   std::string Transformer::getFramePrefix(const std::string& frame_id)
+     578             :   {
+     579        4018 :     const auto firstof = frame_id.find_first_of('/');
+     580        4018 :     if (firstof == std::string::npos)
+     581           0 :       return "";
+     582             :     else
+     583        4018 :       return frame_id.substr(0, firstof+1);
+     584             :   }
+     585             :   //}
+     586             : 
+     587             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/transformer.cpp.gcov.overview.html b/mrs_lib/src/transformer/transformer.cpp.gcov.overview.html new file mode 100644 index 0000000000..18b7b2cf49 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.gcov.overview.html @@ -0,0 +1,167 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/transformer/transformer.cpp.gcov.png b/mrs_lib/src/transformer/transformer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4ff2b0c9fa7b42598266c351feaaee51bdaf01d7 GIT binary patch literal 1995 zcmV;+2Q>JJP))dTBU|#LNWnRdg4=R%6}JAzmUEkiX&NUtZheNG zYM9ax12#c(z)?P90~i2xlySpzaX6;Ou4{Qd=6L4N88JCs@z*8Q`P6{TS-{59Pt4c` zR5095(o}^s1R$8>0`U2MH}^37n$Ly0%l+pdX!6VEv8^VsfH_@efJ;-abql#&~qdAW*wZ=Oy_8QPvLRrh|Uq9 z3`wg!Sic_Ntl-_|KkNZ8*Z{9)GWAgldvGNPE5ewOhKQQW?v5mb01G(Ga57^Fy68oi z>l8}FAv$V8+*iX{z<1E_>lL?BoYeuZ({QBci%!k13m?PNRR@f@MzeuT*hJHKrE$5U zH0+)6#LR>a14Iywz&Kj<;^?{{VZSh^=%yHRWFZ-ab$^z}wcghL_+LFP^#Qr>)j3jH zE|*wZ4FEt>;Fnn?mNhGGi{rhbr(wYiM}<;IWZk{`_*&s7T4Aqr#)XB!hXx5TrcAa> z(iGzyfY1uXlBxL~v>}hKGfrK1Q@$Se6Fn}m z)cbRJPm=ZlphrwFV}HP^vo};Vsgq(9iFH!N(mZwn@P$t5>i|B{0en-gzn5|)hA>Ni z9Vv^W++5!y+e@7nW8ChFjTld~a`eL$pYGvtdV%WMM`^#BP3ar7#2qx{&2zR!L+Jnz z?10s98IHsO&|2y@0c)LMFtkx&jB3$jJ1QX4%1g=7EGjSzmpg#fLv25Q_J@D*2>|sB z$Um|xpOp2zaGxch;qxeUydI;mI@wgsl;UWbA^Bp!1AXMUhiB}5KznhUM3)l3)$|p4 z8~Lbmq`|Cx0z~?Z(bB~ZQeBeC0VI8J&Y%<_asa%nt2sp#@EvlB{%UsNSzO;esbW0U z{qTXk3Q(r_cAStzx(_}!5772ZqN_=P_&j@N;_cIsbbM$0O4MAtNJh<`2})S++ahnG zLyPS3tZCv=^*xoAA#EmQu^UkCA7Zx^<`{ara~zvks`wak1zdOpMun1p^x8wW74qs3 zgzEvzP*8Bl0ZKLjTt}tEx||hmaaYHGWw$RI?HVSE8}O|13(5L`j|!WK8payLuZu8B zuqncLD+OMERfM$f8;fw^4E6#MibvrdnG(R=HZaRQszz|2OSZH)vMa7Ec_o3(F8EOp z)7Zu_^i^jHM8lS6O~K-5%rWp~aR!VJimzB8V?JUgvgMw_0rb@hJMFQs@5Ta(ki@=+ z6}DXnlp_DJtm z3i&t_2WirQMf&KxCds6TLq%_vYeXo_T;HZY{DUi`ekA}Q2HQ<~aYKF?=p9M`NA-?~ zrI(@LLY7bBT;i79mdg1A*&Yl|DSDRxD^lFO5#h)$eFDc=P(%b%B{WWSPb!_!E;%j;N4}X%*(M=sOnaZ> z{o(MCG<&d8ZZqMn=IKxIQohgPn5D_u?EWJ~68`01@!GS*3%1Rxr+&kiNrLIpuj%=q zPc6zST=f3Tb7x@6V?bBWY>k*xefUcaoARM0dp(9hh!Z3q-9ua@nrF=RIlw7~HB*TJR8s=$PS0w#sWPRsYPn2RwGOrQ?ld(kM zsBm}f002ovPDHLkV1hv?z6byS literal 0 HcmV?d00001 diff --git a/mrs_lib/src/utils/index-detail-sort-f.html b/mrs_lib/src/utils/index-detail-sort-f.html new file mode 100644 index 0000000000..e5dc2185f9 --- /dev/null +++ b/mrs_lib/src/utils/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
utils.cpp +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
<unnamed>100.0 %7 / 7100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/index-detail-sort-l.html b/mrs_lib/src/utils/index-detail-sort-l.html new file mode 100644 index 0000000000..e47d684c16 --- /dev/null +++ b/mrs_lib/src/utils/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
utils.cpp +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
<unnamed>100.0 %7 / 7100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/index-detail.html b/mrs_lib/src/utils/index-detail.html new file mode 100644 index 0000000000..12160fc5b3 --- /dev/null +++ b/mrs_lib/src/utils/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
utils.cpp +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
<unnamed>100.0 %7 / 7100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/index-sort-f.html b/mrs_lib/src/utils/index-sort-f.html new file mode 100644 index 0000000000..b80be9ea12 --- /dev/null +++ b/mrs_lib/src/utils/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
utils.cpp +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/index-sort-l.html b/mrs_lib/src/utils/index-sort-l.html new file mode 100644 index 0000000000..5898775a04 --- /dev/null +++ b/mrs_lib/src/utils/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
utils.cpp +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/index.html b/mrs_lib/src/utils/index.html new file mode 100644 index 0000000000..2b4d18bd72 --- /dev/null +++ b/mrs_lib/src/utils/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
utils.cpp +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/utils.cpp.func-sort-c.html b/mrs_lib/src/utils/utils.cpp.func-sort-c.html new file mode 100644 index 0000000000..613c60e277 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>&)529397
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()529405
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/utils.cpp.func.html b/mrs_lib/src/utils/utils.cpp.func.html new file mode 100644 index 0000000000..37871a3f4e --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>&)529397
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()529405
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/utils.cpp.gcov.frameset.html b/mrs_lib/src/utils/utils.cpp.gcov.frameset.html new file mode 100644 index 0000000000..7f64122dba --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/src/utils/utils.cpp.gcov.html b/mrs_lib/src/utils/utils.cpp.gcov.html new file mode 100644 index 0000000000..6190709e28 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.gcov.html @@ -0,0 +1,101 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-04-25 05:42:57Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/utils.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6      529397 : AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>& in)
+       7      529397 :   : variable(in)
+       8             : {
+       9      529397 :   variable = true;
+      10      529403 : }
+      11             : 
+      12     1058810 : AtomicScopeFlag::~AtomicScopeFlag()
+      13             : {
+      14      529405 :   variable = false;
+      15      529405 : }
+      16             : 
+      17             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/utils.cpp.gcov.overview.html b/mrs_lib/src/utils/utils.cpp.gcov.overview.html new file mode 100644 index 0000000000..f3b2e0a717 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.gcov.overview.html @@ -0,0 +1,25 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp + + + + + + + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/utils/utils.cpp.gcov.png b/mrs_lib/src/utils/utils.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3b82aa0d55e194f12f9515f4d5ef4f00538985ef GIT binary patch literal 194 zcmeAS@N?(olHy`uVBq!ia0vp^0YEIs!VDyLSeu0dDTx4|5ZC|z|E~gqdj0}#k_Vn~lnZVFINzf&xlOe%CX$e!F(g};y)F!t{2}&Ee3~RkNUNtx`aKVxJ gJcj|7z|;l?m3NMIqUuuIK(iS + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()2
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()19
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)24
(anonymous namespace)::ProxyExec0::ProxyExec0()31
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()31
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)47
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)63
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()77
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3113
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()9790
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()9790
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()9790
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()9942
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()9942
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()9942
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)18482
mrs_uav_autostart::automatic_start::Topic::getTime()39237
mrs_uav_autostart::automatic_start::Topic::updateTime()48577
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()::{lambda(boost::shared_ptr<topic_tools::ShapeShifter const> const&)#1}::operator()(boost::shared_ptr<topic_tools::ShapeShifter const> const&) const48580
mrs_uav_autostart::automatic_start::AutomaticStart::genericCallback(boost::shared_ptr<topic_tools::ShapeShifter const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int)48583
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)119633
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.func.html b/mrs_uav_autostart/src/automatic_start.cpp.func.html new file mode 100644 index 0000000000..5ae09be5bb --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.func.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()31
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()9790
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)47
mrs_uav_autostart::automatic_start::AutomaticStart::genericCallback(boost::shared_ptr<topic_tools::ShapeShifter const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int)48583
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()9942
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()9790
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()9790
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)119633
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()9942
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()9942
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)24
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3113
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()2
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()31
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()19
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)18482
mrs_uav_autostart::automatic_start::Topic::updateTime()48577
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()77
mrs_uav_autostart::automatic_start::Topic::getTime()39237
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)63
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()::{lambda(boost::shared_ptr<topic_tools::ShapeShifter const> const&)#1}::operator()(boost::shared_ptr<topic_tools::ShapeShifter const> const&) const48580
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.gcov.frameset.html b/mrs_uav_autostart/src/automatic_start.cpp.gcov.frameset.html new file mode 100644 index 0000000000..dc21d0ec86 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.gcov.html b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html new file mode 100644 index 0000000000..e2ee73f457 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html @@ -0,0 +1,1049 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <nodelet/nodelet.h>
+       5             : 
+       6             : #include <mrs_lib/param_loader.h>
+       7             : #include <mrs_lib/mutex.h>
+       8             : #include <mrs_lib/subscribe_handler.h>
+       9             : #include <mrs_lib/publisher_handler.h>
+      10             : 
+      11             : #include <std_msgs/Bool.h>
+      12             : 
+      13             : #include <std_srvs/Trigger.h>
+      14             : #include <std_srvs/SetBool.h>
+      15             : 
+      16             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      17             : #include <mrs_msgs/UavManagerDiagnostics.h>
+      18             : #include <mrs_msgs/ValidateReference.h>
+      19             : #include <mrs_msgs/GazeboSpawnerDiagnostics.h>
+      20             : #include <mrs_msgs/HwApiStatus.h>
+      21             : #include <mrs_msgs/HwApiCapabilities.h>
+      22             : #include <mrs_msgs/EstimationDiagnostics.h>
+      23             : 
+      24             : #include <sensor_msgs/Range.h>
+      25             : #include <sensor_msgs/Imu.h>
+      26             : 
+      27             : #include <topic_tools/shape_shifter.h>
+      28             : 
+      29             : //}
+      30             : 
+      31             : namespace mrs_uav_autostart
+      32             : {
+      33             : 
+      34             : namespace automatic_start
+      35             : {
+      36             : 
+      37             : /* class Topic //{ */
+      38             : 
+      39             : class Topic {
+      40             : private:
+      41             :   std::string topic_name_;
+      42             :   ros::Time   last_time_;
+      43             : 
+      44             : public:
+      45          63 :   Topic(std::string topic_name) : topic_name_(topic_name) {
+      46          63 :     last_time_ = ros::Time::UNINITIALIZED;
+      47          63 :   }
+      48             : 
+      49       48577 :   void updateTime(void) {
+      50       48577 :     last_time_ = ros::Time::now();
+      51       48585 :   }
+      52             : 
+      53       39237 :   ros::Time getTime(void) {
+      54       39237 :     return last_time_;
+      55             :   }
+      56             : 
+      57          77 :   std::string getTopicName(void) {
+      58          77 :     return topic_name_;
+      59             :   }
+      60             : };
+      61             : 
+      62             : //}
+      63             : 
+      64             : /* class AutomaticStart //{ */
+      65             : 
+      66             : // state machine
+      67             : typedef enum
+      68             : {
+      69             :   STATE_IDLE,
+      70             :   STATE_TAKEOFF,
+      71             :   STATE_FINISHED
+      72             : } LandingStates_t;
+      73             : 
+      74             : const char* state_names[3] = {"IDLING", "TAKEOFF", "FINISHED"};
+      75             : 
+      76             : class AutomaticStart : public nodelet::Nodelet {
+      77             : 
+      78             : public:
+      79             :   virtual void onInit();
+      80             : 
+      81             : private:
+      82             :   ros::NodeHandle   nh_;
+      83             :   std::atomic<bool> is_initialized_ = false;
+      84             : 
+      85             :   std::string _uav_name_;
+      86             :   bool        _simulation_;
+      87             : 
+      88             :   // | --------------------- service clients -------------------- |
+      89             : 
+      90             :   ros::ServiceClient service_client_toggle_control_output_;
+      91             :   ros::ServiceClient service_client_arm_;
+      92             :   ros::ServiceClient service_client_takeoff_;
+      93             :   ros::ServiceClient service_client_validate_reference_;
+      94             : 
+      95             :   // | ----------------------- subscribers ---------------------- |
+      96             : 
+      97             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>     sh_estimation_diag_;
+      98             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>               sh_hw_api_status_;
+      99             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>         sh_hw_api_capabilities_;
+     100             :   mrs_lib::SubscribeHandler<sensor_msgs::Range>                  sh_distance_sensor_;
+     101             :   mrs_lib::SubscribeHandler<sensor_msgs::Imu>                    sh_imu_;
+     102             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+     103             :   mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>     sh_uav_manager_diag_;
+     104             :   mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>  sh_gazebo_spawner_diag_;
+     105             : 
+     106             :   // | ----------------------- publishers ----------------------- |
+     107             : 
+     108             :   mrs_lib::PublisherHandler<std_msgs::Bool> ph_can_takeoff_;
+     109             : 
+     110             :   // | ----------------------- main timer ----------------------- |
+     111             : 
+     112             :   ros::Timer timer_main_;
+     113             :   void       timerMain(const ros::TimerEvent& event);
+     114             :   double     _main_timer_rate_;
+     115             : 
+     116             :   // | ------------------- hw api diagnostics ------------------- |
+     117             : 
+     118             :   void              callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     119             :   std::atomic<bool> hw_api_connected_ = false;
+     120             :   std::mutex        mutex_hw_api_status_;
+     121             : 
+     122             :   // | --------------- Gazebo spawner diagnostics --------------- |
+     123             : 
+     124             :   void                               callbackGazeboSpawnerDiagnostics(const mrs_msgs::GazeboSpawnerDiagnostics::ConstPtr msg);
+     125             :   std::atomic<bool>                  got_gazebo_spawner_diagnostics = false;
+     126             :   mrs_msgs::GazeboSpawnerDiagnostics gazebo_spawner_diagnostics_;
+     127             :   std::mutex                         mutex_gazebo_spawner_diagnostics_;
+     128             : 
+     129             :   // | ----------------- arm and offboard check ----------------- |
+     130             : 
+     131             :   ros::Time armed_time_;
+     132             :   bool      armed_ = false;
+     133             : 
+     134             :   ros::Time offboard_time_;
+     135             :   bool      offboard_ = false;
+     136             : 
+     137             :   bool we_toggled_output_ = false;
+     138             : 
+     139             :   // | ------------------------ routines ------------------------ |
+     140             : 
+     141             :   bool takeoff();
+     142             : 
+     143             :   bool validateReference();
+     144             : 
+     145             :   bool toggleControlOutput(const bool& value);
+     146             :   bool disarm();
+     147             : 
+     148             :   bool isGazeboSimulation(void);
+     149             :   bool topicCheck(void);
+     150             :   bool preflightCheckSpeed(void);
+     151             :   bool preflighCheckHeight(void);
+     152             :   bool preflighCheckGyro(void);
+     153             : 
+     154             :   bool is_gazebo_simulation_ = false;
+     155             : 
+     156             :   // | ---------------------- other params ---------------------- |
+     157             : 
+     158             :   std::string _body_frame_name_;
+     159             :   double      _pre_takeoff_sleep_;
+     160             :   bool        _handle_takeoff_ = false;
+     161             :   double      _safety_timeout_;
+     162             :   double      _control_output_timeout_;
+     163             : 
+     164             :   // | ---------------------- state machine --------------------- |
+     165             : 
+     166             :   uint current_state = STATE_IDLE;
+     167             :   void changeState(LandingStates_t new_state);
+     168             : 
+     169             :   // | --------------------- preflight check -------------------- |
+     170             : 
+     171             :   double _preflight_check_time_window_;
+     172             : 
+     173             :   // | ------------------ preflight speed check ----------------- |
+     174             : 
+     175             :   bool      _speed_check_enabled_ = false;
+     176             :   double    _speed_check_max_speed_;
+     177             :   ros::Time speed_check_violated_time_;
+     178             : 
+     179             :   // | ----------------- preflight height check ----------------- |
+     180             : 
+     181             :   bool      _height_check_enabled_ = false;
+     182             :   double    _height_check_max_height_;
+     183             :   ros::Time height_check_violated_time_;
+     184             : 
+     185             :   // | ----------------- preflight gyro check ----------------- |
+     186             : 
+     187             :   bool      _gyro_check_enabled_ = false;
+     188             :   double    _gyro_check_max_rate_;
+     189             :   ros::Time gyro_check_violated_time_;
+     190             : 
+     191             :   // | ---------------- generic topic subscribers --------------- |
+     192             : 
+     193             :   bool                     _topic_check_enabled_ = false;
+     194             :   double                   _topic_check_timeout_;
+     195             :   std::vector<std::string> _topic_check_topic_names_;
+     196             : 
+     197             :   std::vector<Topic>           topic_check_topics_;
+     198             :   std::vector<ros::Subscriber> generic_subscriber_vec_;
+     199             : 
+     200             :   // generic callback, for any topic, to monitor its rate
+     201             :   void genericCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string& topic_name, const int id);
+     202             : };
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* onInit() //{ */
+     207             : 
+     208          31 : void AutomaticStart::onInit() {
+     209             : 
+     210          31 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     211             : 
+     212          31 :   ros::Time::waitForValid();
+     213             : 
+     214          31 :   armed_      = false;
+     215          31 :   armed_time_ = ros::Time(0);
+     216             : 
+     217          31 :   offboard_      = false;
+     218          31 :   offboard_time_ = ros::Time(0);
+     219             : 
+     220          62 :   mrs_lib::ParamLoader param_loader(nh_, "AutomaticStart");
+     221             : 
+     222          62 :   std::string custom_config_path;
+     223             : 
+     224          31 :   param_loader.loadParam("custom_config", custom_config_path);
+     225             : 
+     226          31 :   if (custom_config_path != "") {
+     227           5 :     param_loader.addYamlFile(custom_config_path);
+     228             :   }
+     229             : 
+     230          31 :   param_loader.addYamlFileFromParam("config_private");
+     231          31 :   param_loader.addYamlFileFromParam("config_public");
+     232             : 
+     233          31 :   param_loader.loadParam("uav_name", _uav_name_);
+     234          31 :   param_loader.loadParam("simulation", _simulation_);
+     235             : 
+     236          31 :   param_loader.loadParam("main_timer_rate", _main_timer_rate_);
+     237          31 :   param_loader.loadParam("body_frame_name", _body_frame_name_);
+     238          31 :   param_loader.loadParam("control_output_timeout", _control_output_timeout_);
+     239             : 
+     240          31 :   param_loader.loadParam("safety_timeout", _safety_timeout_);
+     241          31 :   param_loader.loadParam("pre_takeoff_sleep", _pre_takeoff_sleep_);
+     242             : 
+     243          31 :   param_loader.loadParam("handle_takeoff", _handle_takeoff_);
+     244             : 
+     245          31 :   param_loader.loadParam("preflight_check/time_window", _preflight_check_time_window_);
+     246             : 
+     247          31 :   param_loader.loadParam("preflight_check/speed_check/enabled", _speed_check_enabled_);
+     248          31 :   param_loader.loadParam("preflight_check/speed_check/max_speed", _speed_check_max_speed_);
+     249             : 
+     250          31 :   param_loader.loadParam("preflight_check/height_check/enabled", _height_check_enabled_);
+     251          31 :   param_loader.loadParam("preflight_check/height_check/max_height", _height_check_max_height_);
+     252             : 
+     253          31 :   param_loader.loadParam("preflight_check/gyro_check/enabled", _gyro_check_enabled_);
+     254          31 :   param_loader.loadParam("preflight_check/gyro_check/max_rate", _gyro_check_max_rate_);
+     255             : 
+     256          31 :   param_loader.loadParam("preflight_check/topic_check/enabled", _topic_check_enabled_);
+     257          31 :   param_loader.loadParam("preflight_check/topic_check/timeout", _topic_check_timeout_);
+     258          31 :   param_loader.loadParam("preflight_check/topic_check/topics", _topic_check_topic_names_);
+     259             : 
+     260          31 :   if (!param_loader.loadedSuccessfully()) {
+     261           0 :     ROS_ERROR("[AutomaticStart]: Could not load all parameters!");
+     262           0 :     ros::shutdown();
+     263             :   }
+     264             : 
+     265             :   // | ----------------------- subscribers ---------------------- |
+     266             : 
+     267          62 :   mrs_lib::SubscribeHandlerOptions shopts;
+     268          31 :   shopts.nh                 = nh_;
+     269          31 :   shopts.node_name          = "AutomaticStart";
+     270          31 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     271          31 :   shopts.threadsafe         = true;
+     272          31 :   shopts.autostart          = true;
+     273          31 :   shopts.queue_size         = 10;
+     274          31 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     275             : 
+     276          31 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diag_in");
+     277          31 :   sh_hw_api_status_        = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &AutomaticStart::callbackHwApiStatus, this);
+     278          31 :   sh_hw_api_capabilities_  = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     279          31 :   sh_distance_sensor_      = mrs_lib::SubscribeHandler<sensor_msgs::Range>(shopts, "distance_sensor_in");
+     280          31 :   sh_imu_                  = mrs_lib::SubscribeHandler<sensor_msgs::Imu>(shopts, "imu_in");
+     281          31 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     282          31 :   sh_uav_manager_diag_     = mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>(shopts, "uav_manager_diagnostics_in");
+     283          62 :   sh_gazebo_spawner_diag_  = mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>(shopts, "gazebo_spawner_diagnostics_in",
+     284          31 :                                                                                           &AutomaticStart::callbackGazeboSpawnerDiagnostics, this);
+     285             : 
+     286             :   // | ----------------------- publishers ----------------------- |
+     287             : 
+     288          31 :   ph_can_takeoff_ = mrs_lib::PublisherHandler<std_msgs::Bool>(nh_, "can_takeoff_out");
+     289             : 
+     290             :   // | --------------------- service clients -------------------- |
+     291             : 
+     292          31 :   service_client_takeoff_               = nh_.serviceClient<std_srvs::Trigger>("takeoff_out");
+     293          31 :   service_client_toggle_control_output_ = nh_.serviceClient<std_srvs::SetBool>("toggle_control_output_out");
+     294          31 :   service_client_arm_                   = nh_.serviceClient<std_srvs::SetBool>("arm_out");
+     295             : 
+     296          31 :   service_client_validate_reference_ = nh_.serviceClient<mrs_msgs::ValidateReference>("validate_reference_out");
+     297             : 
+     298             :   // | ------------------ setup generic topics ------------------ |
+     299             : 
+     300          31 :   if (_topic_check_enabled_) {
+     301             : 
+     302          62 :     boost::function<void(const topic_tools::ShapeShifter::ConstPtr&)> callback;
+     303             : 
+     304          94 :     for (int i = 0; i < int(_topic_check_topic_names_.size()); i++) {
+     305             : 
+     306         126 :       std::string topic_name = _topic_check_topic_names_.at(i);
+     307             : 
+     308          63 :       if (topic_name.at(0) != '/') {
+     309          63 :         topic_name = "/" + _uav_name_ + "/" + topic_name;
+     310             :       }
+     311             : 
+     312         126 :       Topic tmp_topic(topic_name);
+     313          63 :       topic_check_topics_.push_back(tmp_topic);
+     314             : 
+     315          63 :       int id = i;  // id to identify which topic called the generic callback
+     316             : 
+     317       48643 :       callback                       = [this, topic_name, id](const topic_tools::ShapeShifter::ConstPtr& msg) -> void { genericCallback(msg, topic_name, id); };
+     318         189 :       ros::Subscriber tmp_subscriber = nh_.subscribe(topic_name, 1, callback);
+     319             : 
+     320          63 :       generic_subscriber_vec_.push_back(tmp_subscriber);
+     321             :     }
+     322             :   }
+     323             : 
+     324             :   // | ------------------------- timers ------------------------- |
+     325             : 
+     326          31 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &AutomaticStart::timerMain, this);
+     327             : 
+     328             :   // | --------------------- finish the init -------------------- |
+     329             : 
+     330          31 :   is_initialized_ = true;
+     331             : 
+     332          31 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: initialized");
+     333          31 : }
+     334             : 
+     335             : //}
+     336             : 
+     337             : // --------------------------------------------------------------
+     338             : // |                          callbacks                         |
+     339             : // --------------------------------------------------------------
+     340             : 
+     341             : /* genericCallback() //{ */
+     342             : 
+     343       48583 : void AutomaticStart::genericCallback([[maybe_unused]] const topic_tools::ShapeShifter::ConstPtr& msg, [[maybe_unused]] const std::string& topic_name,
+     344             :                                      const int id) {
+     345       48583 :   topic_check_topics_.at(id).updateTime();
+     346       48585 : }
+     347             : 
+     348             : //}
+     349             : 
+     350             : /* callbackHwApiDiag() //{ */
+     351             : 
+     352      119633 : void AutomaticStart::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+     353             : 
+     354      119633 :   if (!is_initialized_) {
+     355           0 :     return;
+     356             :   }
+     357             : 
+     358      119633 :   ROS_INFO_ONCE("[AutomaticStart]: getting HW API diagnostics");
+     359             : 
+     360      239266 :   std::scoped_lock lock(mutex_hw_api_status_);
+     361             : 
+     362             :   // check armed_ state
+     363      119633 :   if (armed_ == false) {
+     364             : 
+     365             :     // if armed_ state changed to true, please "start the clock"
+     366       67312 :     if (msg->armed) {
+     367             : 
+     368          30 :       armed_      = true;
+     369          30 :       armed_time_ = ros::Time::now();
+     370             :     }
+     371             : 
+     372             :     // if we were armed_ previously
+     373       52321 :   } else if (armed_ == true) {
+     374             : 
+     375             :     // and we are not really now
+     376       52321 :     if (!msg->armed) {
+     377             : 
+     378           4 :       armed_ = false;
+     379             :     }
+     380             :   }
+     381             : 
+     382             :   // check offboard_ state
+     383      119633 :   if (offboard_ == false) {
+     384             : 
+     385             :     // if offboard_ state changed to true, please "start the clock"
+     386       77620 :     if (msg->offboard) {
+     387             : 
+     388          22 :       offboard_      = true;
+     389          22 :       offboard_time_ = ros::Time::now();
+     390             :     }
+     391             : 
+     392             :     // if we were in offboard_ previously
+     393       42013 :   } else if (offboard_ == true) {
+     394             : 
+     395             :     // and we are not really now
+     396       42013 :     if (!msg->offboard) {
+     397             : 
+     398           0 :       offboard_ = false;
+     399             :     }
+     400             :   }
+     401             : 
+     402      119633 :   if (msg->connected) {
+     403      116485 :     hw_api_connected_ = true;
+     404             :   }
+     405             : }
+     406             : 
+     407             : //}
+     408             : 
+     409             : /* callbackGazeboSpawnerDiagnostics() //{ */
+     410             : 
+     411        3113 : void AutomaticStart::callbackGazeboSpawnerDiagnostics(const mrs_msgs::GazeboSpawnerDiagnostics::ConstPtr msg) {
+     412             : 
+     413        3113 :   if (!is_initialized_) {
+     414           0 :     return;
+     415             :   }
+     416             : 
+     417        3113 :   ROS_INFO_ONCE("[AutomaticStart]: getting spawner diagnostics");
+     418             : 
+     419             :   {
+     420        6226 :     std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     421             : 
+     422        3113 :     gazebo_spawner_diagnostics_ = *msg;
+     423             : 
+     424        3113 :     got_gazebo_spawner_diagnostics = true;
+     425             :   }
+     426             : }
+     427             : 
+     428             : //}
+     429             : 
+     430             : // --------------------------------------------------------------
+     431             : // |                           timers                           |
+     432             : // --------------------------------------------------------------
+     433             : 
+     434             : /* timerMain() //{ */
+     435             : 
+     436       18482 : void AutomaticStart::timerMain([[maybe_unused]] const ros::TimerEvent& event) {
+     437             : 
+     438       18482 :   if (!is_initialized_) {
+     439        5825 :     return;
+     440             :   }
+     441             : 
+     442       18482 :   bool got_uav_manager_diag     = sh_uav_manager_diag_.hasMsg();
+     443       18482 :   bool got_control_manager_diag = sh_control_manager_diag_.hasMsg();
+     444       18482 :   bool got_estimation_diag      = sh_estimation_diag_.hasMsg();
+     445       18482 :   bool got_hw_api               = sh_hw_api_status_.hasMsg() && sh_hw_api_capabilities_.hasMsg() && hw_api_connected_;
+     446             : 
+     447       18482 :   if (!got_control_manager_diag || !got_hw_api || !got_uav_manager_diag || !got_estimation_diag) {
+     448        5673 :     ROS_WARN_THROTTLE(5.0, "[AutomaticStart]: waiting for data: ControlManager=%s, UavManager=%s, HW Api=%s, EstimationManager=%s",
+     449             :                       got_control_manager_diag ? "true" : "FALSE", got_uav_manager_diag ? "true" : "FALSE", got_hw_api ? "true" : "FALSE",
+     450             :                       got_estimation_diag ? "true" : "FALSE");
+     451        5673 :     return;
+     452             :   }
+     453             : 
+     454       12809 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     455       12809 :   auto control_manager_diagnostics                  = sh_control_manager_diag_.getMsg();
+     456             : 
+     457       12809 :   switch (current_state) {
+     458             : 
+     459        9942 :     case STATE_IDLE: {
+     460             : 
+     461             :       // | --------------------- preflight check -------------------- |
+     462             : 
+     463        9942 :       bool speed_valid  = preflightCheckSpeed();
+     464        9942 :       bool height_valid = preflighCheckHeight();
+     465        9942 :       bool gyros_valid  = preflighCheckGyro();
+     466             : 
+     467        9942 :       bool possibly_in_the_air = !speed_valid || !height_valid || !gyros_valid;
+     468             : 
+     469        9942 :       if (possibly_in_the_air) {
+     470             : 
+     471         152 :         ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: preflight check failed, the UAV is possibly in the air");
+     472             : 
+     473         152 :         if (armed) {
+     474             : 
+     475           6 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: -- the UAV is also armed!! finishing to prevent unwanted system activation");
+     476             : 
+     477           6 :           if (we_toggled_output_) {
+     478             : 
+     479           1 :             bool res = toggleControlOutput(false);
+     480             : 
+     481           1 :             if (!res) {
+     482           0 :               ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output OFF");
+     483             :             }
+     484             :           }
+     485             : 
+     486           6 :           changeState(STATE_FINISHED);
+     487             : 
+     488         152 :           return;
+     489             :         }
+     490             : 
+     491         146 :         return;
+     492             :       }
+     493             : 
+     494             :       // | -------------------- ready to takeoff -------------------- |
+     495             : 
+     496        9790 :       bool control_output_enabled = sh_control_manager_diag_.getMsg()->output_enabled;
+     497             : 
+     498        9790 :       std_msgs::Bool can_takeoff_msg;
+     499        9790 :       can_takeoff_msg.data = false;
+     500             : 
+     501             :       // | -------------------- preflight checks -------------------- |
+     502             : 
+     503        9790 :       bool position_valid = validateReference();
+     504        9790 :       bool got_topics     = topicCheck();
+     505             : 
+     506        9790 :       bool can_takeoff = got_topics && position_valid;
+     507             : 
+     508             :       // | ---------------------------------------------------------- |
+     509             : 
+     510        9790 :       can_takeoff_msg.data = can_takeoff;
+     511        9790 :       ph_can_takeoff_.publish(can_takeoff_msg);
+     512             : 
+     513        9790 :       if (armed && !control_output_enabled) {
+     514             : 
+     515         116 :         if (can_takeoff) {
+     516             : 
+     517          23 :           bool res = toggleControlOutput(true);
+     518             : 
+     519          23 :           if (!res) {
+     520           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON");
+     521             :           } else {
+     522          23 :             we_toggled_output_ = true;
+     523             :           }
+     524             :         }
+     525             : 
+     526         116 :         double time_from_arming = (ros::Time::now() - armed_time).toSec();
+     527             : 
+     528         116 :         if (armed_time != ros::Time::UNINITIALIZED && time_from_arming > _control_output_timeout_) {
+     529             : 
+     530           2 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON for %.2f secs, disarming", _control_output_timeout_);
+     531           2 :           disarm();
+     532           2 :           changeState(STATE_FINISHED);
+     533             :         }
+     534             :       }
+     535             : 
+     536        9790 :       if (_simulation_ && isGazeboSimulation()) {
+     537             : 
+     538        6041 :         std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     539             : 
+     540        6041 :         if (got_gazebo_spawner_diagnostics) {
+     541             : 
+     542        6041 :           if (!gazebo_spawner_diagnostics_.spawn_called || gazebo_spawner_diagnostics_.processing) {
+     543           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) waiting for spawner to finish spawning UAVs");
+     544           0 :             return;
+     545             :           }
+     546             : 
+     547             :         } else {
+     548             : 
+     549           0 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) missing spawner diagnostics");
+     550           0 :           return;
+     551             :         }
+     552             :       }
+     553             : 
+     554             :       // when armed and in offboard, takeoff
+     555        9790 :       if (armed && offboard && control_output_enabled) {
+     556             : 
+     557        3262 :         ros::Duration armed_time_diff    = ros::Time::now() - armed_time;
+     558        3262 :         ros::Duration offboard_time_diff = ros::Time::now() - offboard_time;
+     559             : 
+     560        3262 :         if (armed_time_diff > ros::Duration(_safety_timeout_) && offboard_time_diff > ros::Duration(_safety_timeout_)) {
+     561             : 
+     562          20 :           if (_handle_takeoff_) {
+     563          19 :             changeState(STATE_TAKEOFF);
+     564             :           } else {
+     565           1 :             changeState(STATE_FINISHED);
+     566             :           }
+     567             : 
+     568             :         } else {
+     569             : 
+     570        3242 :           double min = (armed_time_diff < offboard_time_diff) ? armed_time_diff.toSec() : offboard_time_diff.toSec();
+     571             : 
+     572        3242 :           ROS_WARN_THROTTLE(1.0, "taking off in %.0f", (_safety_timeout_ - min));
+     573             :         }
+     574             :       }
+     575             : 
+     576        9790 :       break;
+     577             :     }
+     578             : 
+     579        2839 :     case STATE_TAKEOFF: {
+     580             : 
+     581             :       // if takeoff finished
+     582        2839 :       if (control_manager_diagnostics->flying_normally) {
+     583             : 
+     584          19 :         ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: takeoff finished");
+     585             : 
+     586          19 :         changeState(STATE_FINISHED);
+     587             : 
+     588             :       } else {
+     589             : 
+     590        2820 :         ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: waiting for the takeoff to finish");
+     591             :       }
+     592             : 
+     593        2839 :       break;
+     594             :     }
+     595             : 
+     596          28 :     case STATE_FINISHED: {
+     597             : 
+     598          28 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: finished");
+     599          28 :       ros::requestShutdown();
+     600          28 :       break;
+     601             :     }
+     602             :   }
+     603             : }
+     604             : 
+     605             : //}
+     606             : 
+     607             : // --------------------------------------------------------------
+     608             : // |                          routines                          |
+     609             : // --------------------------------------------------------------
+     610             : 
+     611             : /* changeState() //{ */
+     612             : 
+     613          47 : void AutomaticStart::changeState(LandingStates_t new_state) {
+     614             : 
+     615          47 :   ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: switching states %s -> %s", state_names[current_state], state_names[new_state]);
+     616             : 
+     617          47 :   switch (new_state) {
+     618             : 
+     619           0 :     case STATE_IDLE: {
+     620             : 
+     621           0 :       break;
+     622             :     }
+     623             : 
+     624          19 :     case STATE_TAKEOFF: {
+     625             : 
+     626          19 :       if (_pre_takeoff_sleep_ > 1.0) {
+     627           0 :         ROS_INFO("[AutomaticStart]: sleeping for %.2f secs before takeoff", _pre_takeoff_sleep_);
+     628           0 :         ros::Duration(_pre_takeoff_sleep_).sleep();
+     629             :       }
+     630             : 
+     631          19 :       bool res = takeoff();
+     632             : 
+     633          19 :       if (!res) {
+     634             : 
+     635           0 :         current_state = STATE_FINISHED;
+     636             : 
+     637           0 :         return;
+     638             :       }
+     639             : 
+     640          19 :       break;
+     641             :     }
+     642             : 
+     643          28 :     case STATE_FINISHED: {
+     644             : 
+     645          28 :       break;
+     646             :     }
+     647             : 
+     648             :     break;
+     649             :   }
+     650             : 
+     651          47 :   current_state = new_state;
+     652             : }
+     653             : 
+     654             : //}
+     655             : 
+     656             : /* takeoff() //{ */
+     657             : 
+     658          19 : bool AutomaticStart::takeoff() {
+     659             : 
+     660          19 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: taking off");
+     661             : 
+     662          38 :   std_srvs::Trigger srv;
+     663             : 
+     664          19 :   bool res = service_client_takeoff_.call(srv);
+     665             : 
+     666          19 :   if (res) {
+     667             : 
+     668          19 :     if (srv.response.success) {
+     669             : 
+     670          19 :       return true;
+     671             : 
+     672             :     } else {
+     673             : 
+     674           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: taking off failed: %s", srv.response.message.c_str());
+     675             :     }
+     676             : 
+     677             :   } else {
+     678             : 
+     679           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for taking off failed");
+     680             :   }
+     681             : 
+     682           0 :   return false;
+     683             : }
+     684             : 
+     685             : //}
+     686             : 
+     687             : /* validateReference() //{ */
+     688             : 
+     689        9790 : bool AutomaticStart::validateReference() {
+     690             : 
+     691       19580 :   mrs_msgs::ValidateReference srv_out;
+     692             : 
+     693        9790 :   srv_out.request.reference.header.frame_id = _body_frame_name_;
+     694             : 
+     695        9790 :   bool res = service_client_validate_reference_.call(srv_out);
+     696             : 
+     697        9790 :   if (res) {
+     698             : 
+     699        9789 :     if (srv_out.response.success) {
+     700             : 
+     701        9712 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: current position is valid");
+     702        9712 :       return true;
+     703             : 
+     704             :     } else {
+     705             : 
+     706          77 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position is not valid (safety area, bumper)!");
+     707          77 :       return false;
+     708             :     }
+     709             : 
+     710             :   } else {
+     711             : 
+     712           1 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position could not be validated");
+     713           1 :     return false;
+     714             :   }
+     715             : }
+     716             : 
+     717             : //}
+     718             : 
+     719             : /* toggleControlOutput() //{ */
+     720             : 
+     721          24 : bool AutomaticStart::toggleControlOutput(const bool& value) {
+     722             : 
+     723          24 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: setting control output %s", value ? "ON" : "OFF");
+     724             : 
+     725          48 :   std_srvs::SetBool srv;
+     726          24 :   srv.request.data = value;
+     727             : 
+     728          24 :   bool res = service_client_toggle_control_output_.call(srv);
+     729             : 
+     730          24 :   if (res) {
+     731             : 
+     732          24 :     if (srv.response.success) {
+     733             : 
+     734          24 :       return true;
+     735             : 
+     736             :     } else {
+     737             : 
+     738           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: setting of control output failed: %s", srv.response.message.c_str());
+     739             :     }
+     740             : 
+     741             :   } else {
+     742             : 
+     743           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for toggling control output failed");
+     744             :   }
+     745             : 
+     746           0 :   return false;
+     747             : }
+     748             : 
+     749             : //}
+     750             : 
+     751             : /* disarm() //{ */
+     752             : 
+     753           2 : bool AutomaticStart::disarm() {
+     754             : 
+     755           2 :   if (!hw_api_connected_) {
+     756             : 
+     757           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, missing HW API status!");
+     758             : 
+     759           0 :     return false;
+     760             :   }
+     761             : 
+     762           2 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     763             : 
+     764           2 :   if (offboard) {
+     765             : 
+     766           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, already in offboard mode!");
+     767             : 
+     768           0 :     return false;
+     769             :   }
+     770             : 
+     771           2 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: disarming");
+     772             : 
+     773           4 :   std_srvs::SetBool srv;
+     774           2 :   srv.request.data = false;
+     775             : 
+     776           2 :   bool res = service_client_arm_.call(srv);
+     777             : 
+     778           2 :   if (res) {
+     779             : 
+     780           2 :     if (srv.response.success) {
+     781             : 
+     782           2 :       return true;
+     783             : 
+     784             :     } else {
+     785             : 
+     786           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: disarming failed");
+     787             :     }
+     788             : 
+     789             :   } else {
+     790             : 
+     791           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for disarming failed");
+     792             :   }
+     793             : 
+     794           0 :   return false;
+     795             : }
+     796             : 
+     797             : //}
+     798             : 
+     799             : /* isGazeboSimulation() //{ */
+     800             : 
+     801        9790 : bool AutomaticStart::isGazeboSimulation(void) {
+     802             : 
+     803        9790 :   if (is_gazebo_simulation_) {
+     804        6032 :     return true;
+     805             :   }
+     806             : 
+     807        7516 :   ros::V_string node_list;
+     808        3758 :   ros::master::getNodes(node_list);
+     809             : 
+     810       52486 :   for (auto& node : node_list) {
+     811       48737 :     if (node.find("mrs_drone_spawner") != std::string::npos) {
+     812           9 :       ROS_INFO("[AutomaticStart]: MRS Gazebo Simulation detected");
+     813           9 :       is_gazebo_simulation_ = true;
+     814           9 :       return true;
+     815             :     }
+     816             :   }
+     817             : 
+     818        3749 :   return false;
+     819             : }
+     820             : 
+     821             : //}
+     822             : 
+     823             : /* topicCheck() //{ */
+     824             : 
+     825        9790 : bool AutomaticStart::topicCheck(void) {
+     826             : 
+     827        9790 :   bool got_topics = true;
+     828             : 
+     829        9790 :   std::stringstream missing_topics;
+     830             : 
+     831        9790 :   if (_topic_check_enabled_) {
+     832             : 
+     833       29447 :     for (int i = 0; i < int(topic_check_topics_.size()); i++) {
+     834             : 
+     835       39237 :       if (topic_check_topics_.at(i).getTime() == ros::Time::UNINITIALIZED ||
+     836       39237 :           (ros::Time::now() - topic_check_topics_.at(i).getTime()) > ros::Duration(_topic_check_timeout_)) {
+     837             : 
+     838          77 :         missing_topics << std::endl << "\t" << topic_check_topics_.at(i).getTopicName();
+     839          77 :         got_topics = false;
+     840             :       }
+     841             :     }
+     842             :   }
+     843             : 
+     844        9790 :   if (!got_topics) {
+     845          77 :     ROS_WARN_STREAM_THROTTLE(1.0, "[AutomaticStart]: missing data on topics: " << missing_topics.str());
+     846             :   }
+     847             : 
+     848       19580 :   return got_topics;
+     849             : }
+     850             : 
+     851             : //}
+     852             : 
+     853             : // | -------- preflight cheks for detecting flyign UAV -------- |
+     854             : 
+     855             : /* preflightCheckSpeed() //{ */
+     856             : 
+     857        9942 : bool AutomaticStart::preflightCheckSpeed(void) {
+     858             : 
+     859        9942 :   if (!_speed_check_enabled_) {
+     860           0 :     return true;
+     861             :   }
+     862             : 
+     863       19884 :   auto estimation_diag = sh_estimation_diag_.getMsg();
+     864             : 
+     865        9942 :   double speed = std::hypot(estimation_diag->velocity.linear.x, estimation_diag->velocity.linear.y, estimation_diag->velocity.linear.z);
+     866             : 
+     867        9942 :   if (speed > _speed_check_max_speed_) {
+     868           0 :     speed_check_violated_time_ = ros::Time::now();
+     869           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: the estimated speed (%.2f ms^-2) is over the limit (%.2f ms^-2)", speed, _speed_check_max_speed_);
+     870             :   }
+     871             : 
+     872        9942 :   if (speed_check_violated_time_ != ros::Time::UNINITIALIZED &&
+     873        9942 :       (ros::Time::now() - speed_check_violated_time_) < ros::Duration(_preflight_check_time_window_)) {
+     874           0 :     return false;
+     875             :   } else {
+     876        9942 :     return true;
+     877             :   }
+     878             : }
+     879             : 
+     880             : //}
+     881             : 
+     882             : /* preflighCheckHeight() //{ */
+     883             : 
+     884        9942 : bool AutomaticStart::preflighCheckHeight(void) {
+     885             : 
+     886        9942 :   if (!_height_check_enabled_) {
+     887         245 :     return true;
+     888             :   }
+     889             : 
+     890             :   // | ----------------- is the check possible? ----------------- |
+     891             : 
+     892       19394 :   auto capabilities = sh_hw_api_capabilities_.getMsg();
+     893             : 
+     894        9697 :   if (!capabilities->produces_distance_sensor) {
+     895           0 :     return true;
+     896             :   }
+     897             : 
+     898             :   // | -------------------- do we have data? -------------------- |
+     899             : 
+     900        9697 :   if (!sh_distance_sensor_.hasMsg()) {
+     901         669 :     return true;
+     902             :   }
+     903             : 
+     904        9028 :   double height = sh_distance_sensor_.getMsg()->range;
+     905             : 
+     906        9028 :   if (height > _height_check_max_height_) {
+     907         151 :     height_check_violated_time_ = ros::Time::now();
+     908         151 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: the height (%.2f m) is over the limit (%.2f m)", height, _height_check_max_height_);
+     909             :   }
+     910             : 
+     911        9179 :   if (height_check_violated_time_ != ros::Time::UNINITIALIZED &&
+     912        9179 :       (ros::Time::now() - height_check_violated_time_) < ros::Duration(_preflight_check_time_window_)) {
+     913         151 :     return false;
+     914             :   } else {
+     915        8877 :     return true;
+     916             :   }
+     917             : }
+     918             : 
+     919             : //}
+     920             : 
+     921             : /* preflighCheckGyro() //{ */
+     922             : 
+     923        9942 : bool AutomaticStart::preflighCheckGyro(void) {
+     924             : 
+     925        9942 :   if (!_gyro_check_enabled_) {
+     926           0 :     return true;
+     927             :   }
+     928             : 
+     929             :   // | ----------------- is the check possible? ----------------- |
+     930             : 
+     931       19884 :   auto capabilities = sh_hw_api_capabilities_.getMsg();
+     932             : 
+     933        9942 :   if (!capabilities->produces_imu) {
+     934           0 :     return true;
+     935             :   }
+     936             : 
+     937             :   // | -------------------- do we have data? -------------------- |
+     938             : 
+     939        9942 :   if (!sh_imu_.hasMsg()) {
+     940           0 :     return true;
+     941             :   }
+     942             : 
+     943        9942 :   auto gyros = sh_imu_.getMsg()->angular_velocity;
+     944             : 
+     945        9942 :   if (abs(gyros.x) > _gyro_check_max_rate_ || abs(gyros.y) > _gyro_check_max_rate_ || abs(gyros.z) > _gyro_check_max_rate_) {
+     946           1 :     gyro_check_violated_time_ = ros::Time::now();
+     947           1 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: the angular velocity ([%.2f, %.2f, %.2f] rad/s) is over the limit (%.2f rad/s)", gyros.x, gyros.y, gyros.z,
+     948             :                       _gyro_check_max_rate_);
+     949             :   }
+     950             : 
+     951        9942 :   if (gyro_check_violated_time_ != ros::Time::UNINITIALIZED && (ros::Time::now() - gyro_check_violated_time_) < ros::Duration(_preflight_check_time_window_)) {
+     952           1 :     return false;
+     953             :   } else {
+     954        9941 :     return true;
+     955             :   }
+     956             : }
+     957             : 
+     958             : //}
+     959             : 
+     960             : }  // namespace automatic_start
+     961             : 
+     962             : }  // namespace mrs_uav_autostart
+     963             : 
+     964             : #include <pluginlib/class_list_macros.h>
+     965          31 : PLUGINLIB_EXPORT_CLASS(mrs_uav_autostart::automatic_start::AutomaticStart, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html b/mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html new file mode 100644 index 0000000000..ab13fd002f --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html @@ -0,0 +1,262 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.gcov.png b/mrs_uav_autostart/src/automatic_start.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4f46c4db278e7eb51fcddd461ed7a5ce53509936 GIT binary patch literal 3128 zcmV-849D|{P)?ftK^jbBhyRI;7U8)kQQ+NB7nfPk6hw%%^H?ftb# z!JD2TdTWu4hz#V;p)G7I#2$)8bT%Rz*gTF=EGwr2ad7AgX%dm*fIy@fEezd8^#^5}# zrrA+l*JxGOQImM;_1%K!j#BeM4 z>t*Yw2&pBoH4w{Tb4Iar1}F^OErz2A)#J#5cV^T>!LqF3r!tO#Hlo9AAqEi|h*8uv zys8X+0*gnU)Q*RUI5UMCBq=k6bA~hb2np6+29L)R(#p#_48Kr!xB8JgHgZRxTYnwfQEiL@yWt-LnH+pThH z5e-|Zp;?Gs6w4YKM3hMl!-!O3E24$#jEO)H7yU{Vae?Nvg17+0irAQ9J!!vSOFpYe zN{_&~jvD}faIL8aI~GQf= zk(IiF58q;>I%hF>VB>gia}~;Ohkj>sw08iL%T*$1Z3cg z;f0RXSBY5g3X|}(F~8%Y>&?BhX6i}pBZAG@%dg2^%QiK{s!P47(NiQr^(8=joMNLK zHAM0KuWr|8P{FKR2Y=!tQ8--_q=r_~Lm%W~nnVPgdi+TqiA_@&8Z2Ecq8@VS zDT*1R$00%yQEedxOtGPZrkaR;Y_Xxk9CfJYe=D4g2!!GDs4_B?*J zzbxD>0K|J0mix1g5D@twUOCGYerl^Y*FkT((&^$Uc|@o4`s&e?UriCUqgZfx`Or;7 zN3raN56D&?O6q{f1VGWnT?N2AvJg*P9wKAtpc3xT2NO|B7`DQ>i1IZ}FHGSK671P1 zj}n~)Na1c6ie1VylXBr@R;9?E!d!9W${5ol$hodFdW0;M8r`}q1s0o-*l1OEQPijI zlqP3RdB@nbxgsa|tZ0bHrx*`}x~NJ+(T{2e*4nQ~oglK#NF8&1jtn(e6NUi77c#+6 z)Ngt9TQy3%bz%Q#!;-fAbWb68%VYz<0<%m5VZ7*gzAZb)!0MqOLFAosuj%_vOO+^D zJ8E%DJ|Vt6;T5*f?Ej+Xg1OqIt|w^Hkd;$r5(JyD8e?Zu8HoMX_aw49!bUCgBqUGj+vLn z%_gRL{GN$ty|8^>2w{t$f9UWnj-VujDa@ zzS~6Z^V!Jcz7cso?t(qT6dEQA>LEM7UUVRVaWvsU`KVA8sZf_ zA5mTrsO>&Ll^K-2t-`>h&)u(d}OkRq}pP7RdU-y}2)uh)KkxF;5?~zHQIf}>7@x(=h z6epwx6#dFR;?@a!r>oe6_F-{)`YO3VD>#d+Pf6{)FJHCeS+P$_nbNJKV8RuZ|}NTp6;PHK)*@Lr+m6r5Lh|L|ye zh22R5X1l^8Ck{VB_K58`j0wIY`^j)Uv?vrZjq#P4T#<0bgj&AiBqBC++2>ukzEC*# zv4gz@4~_XX1SX)+wU*}^GXP}{hqf!h9(=pLUH$9F>k$zfaooZT_If&{7xP%KiN|ZD zz!TaVDMl)HVG3=ezN8wr*?f5@`Vd$<^jz3~seJXY7p}J4lLl z`J$sjeP|w>L~W6MyV0fuN?jMT%+;p?lW(970%xpVKES0nWqscOm@ z3EN1yer8{)|C=B-uUN%L>VU{ zOFup80r?}5zP+85Kk97ya_V_LX~jhK@K%qb*wp1!3jbYhn1y&j|0DCEOBXcwg`HoO zo__;^4OcmK!DgJ)X-X84OHMPX=?qXTUyARinAK(}liJx5(@H7yQACG0k!LmebhsFg z;zusuGmJBTFXw?>mBJ+frGpP~S(dUa;kg%QfbqJZ;q>lw)}+WBxnt;Wz4%4#-Z8Tm zE(r@#1QD!*A&QjQO9}oUvnSMz17{gdk!>vmH@bhkeZ<0OB$lrrDA)dUWImS`Mm*XR zH>6FWO*T%shSRptzqpZH#z2-Y#wzJtivMeJDSd6^^6pv?Y`r=ICow+Kmxj@IfQ_-n#(bOkH z;jiyeIPI^G?9oANh5synO+BNsgW{jRmVkT3$JuXTEBp?B{rvOSZHlmKU&Lz{nnGt2 zzR^O~HWzCXb{t9BfV%tY!larh3d7`a_h&j?*H4!3IZ#Wmf{1B~Ewq}UP7;h*d00b_ zef-!nU6}cdZz^9&M&kLtrX6Qzs8C-x{S@Io6xm#C+ry<;d2p!$Ivs7ih$=wmrqEmr zw`(@O{m4*nq0)X-opTcr8G$CZ!S7>zWXwli?Mq@uvwW)6lu{QtBzLR2PVqae*?NoD z3O9Jca2>ckxBSmMF0m+>3dg&;a!m1O*Jhx3S+W2aj{w6`+vIntts0pWz8=ocMs;aC1m{H8W_G?!j%vEb<8RH)t~zif^9`+PJn(wQGlfNZ5T+zZ1+*{p5lMP9#!>Y St%8mK0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
<unnamed>88.3 %287 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail-sort-l.html b/mrs_uav_autostart/src/index-detail-sort-l.html new file mode 100644 index 0000000000..ba3e042402 --- /dev/null +++ b/mrs_uav_autostart/src/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
<unnamed>88.3 %287 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail.html b/mrs_uav_autostart/src/index-detail.html new file mode 100644 index 0000000000..c71d44522d --- /dev/null +++ b/mrs_uav_autostart/src/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
<unnamed>88.3 %287 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-f.html b/mrs_uav_autostart/src/index-sort-f.html new file mode 100644 index 0000000000..252e13d5a3 --- /dev/null +++ b/mrs_uav_autostart/src/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-l.html b/mrs_uav_autostart/src/index-sort-l.html new file mode 100644 index 0000000000..2e257fc21e --- /dev/null +++ b/mrs_uav_autostart/src/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index.html b/mrs_uav_autostart/src/index.html new file mode 100644 index 0000000000..efef9a9e96 --- /dev/null +++ b/mrs_uav_autostart/src/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28732588.3 %
Date:2024-04-25 05:42:57Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.3%88.3%
+ 		88.3 %287 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.func-sort-c.html b/mrs_uav_controllers/include/common.h.func-sort-c.html new file mode 100644 index 0000000000..325e4118db --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-04-25 05:42:57Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)4
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)10
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)38
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)40
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.func.html b/mrs_uav_controllers/include/common.h.func.html new file mode 100644 index 0000000000..385fad4dc3 --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-04-25 05:42:57Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)38
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)4
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)40
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)10
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)2
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.gcov.frameset.html b/mrs_uav_controllers/include/common.h.gcov.frameset.html new file mode 100644 index 0000000000..9f1a16783b --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/include/common.h.gcov.html b/mrs_uav_controllers/include/common.h.gcov.html new file mode 100644 index 0000000000..82985d8b8e --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.html @@ -0,0 +1,189 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-04-25 05:42:57Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_UAV_CONTROLLERS_COMMON_H
+       2             : #define MRS_UAV_CONTROLLERS_COMMON_H
+       3             : 
+       4             : #include <eigen3/Eigen/Eigen>
+       5             : #include <mrs_uav_managers/control_manager/common_handlers.h>
+       6             : #include <mrs_uav_managers/controller.h>
+       7             : #include <mrs_lib/attitude_converter.h>
+       8             : 
+       9             : namespace mrs_uav_controllers
+      10             : {
+      11             : 
+      12             : namespace common
+      13             : {
+      14             : 
+      15             : enum CONTROL_OUTPUT
+      16             : {
+      17             :   ACTUATORS_CMD,
+      18             :   CONTROL_GROUP,
+      19             :   ATTITUDE_RATE,
+      20             :   ATTITUDE,
+      21             :   ACCELERATION_HDG_RATE,
+      22             :   ACCELERATION_HDG,
+      23             :   VELOCITY_HDG_RATE,
+      24             :   VELOCITY_HDG,
+      25             :   POSITION
+      26             : };
+      27             : 
+      28             : Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d& Rd);
+      29             : 
+      30             : std::optional<Eigen::Vector3d> sanitizeDesiredForce(const Eigen::Vector3d& desired_force, const double& tilt_over_limit, const double& tilt_saturation,
+      31             :                                                     const std::string& node_name);
+      32             : 
+      33             : Eigen::Matrix3d so3transform(const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading);
+      34             : 
+      35             : std::optional<CONTROL_OUTPUT> getLowestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs);
+      36             : 
+      37             : std::optional<CONTROL_OUTPUT> getHighestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs);
+      38             : 
+      39             : std::optional<mrs_msgs::HwApiAttitudeRateCmd> attitudeController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeCmd& reference,
+      40             :                                                                  const Eigen::Vector3d& ff_rate, const Eigen::Vector3d& rate_saturation,
+      41             :                                                                  const Eigen::Vector3d& gains, const bool& parasitic_heading_rate_compensation);
+      42             : 
+      43             : std::optional<mrs_msgs::HwApiControlGroupCmd> attitudeRateController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeRateCmd& reference,
+      44             :                                                                      const Eigen::Vector3d& gains);
+      45             : 
+      46             : 
+      47             : mrs_msgs::HwApiActuatorCmd actuatorMixer(const mrs_msgs::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer);
+      48             : 
+      49             : /* throttle extraction //{ */
+      50             : 
+      51             : std::optional<double> extractThrottle(const mrs_uav_managers::Controller::ControlOutput& control_output);
+      52             : 
+      53             : struct HwApiCmdExtractThrottleVisitor
+      54             : {
+      55           3 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      56             : 
+      57           3 :     double throttle = 0;
+      58             : 
+      59           3 :     if (msg.motors.size() == 0) {
+      60           0 :       return std::nullopt;
+      61             :     }
+      62             : 
+      63           3 :     throttle = 0;
+      64             : 
+      65          15 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      66          12 :       throttle += msg.motors[i];
+      67             :     };
+      68             : 
+      69           3 :     throttle /= msg.motors.size();
+      70             : 
+      71           3 :     return throttle;
+      72             :   }
+      73           3 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      74           3 :     return msg.throttle;
+      75             :   }
+      76          38 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      77          38 :     return msg.throttle;
+      78             :   }
+      79          40 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      80          40 :     return msg.throttle;
+      81             :   }
+      82           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      83           2 :     return std::nullopt;
+      84             :   }
+      85           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      86           2 :     return std::nullopt;
+      87             :   }
+      88          10 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      89          10 :     return std::nullopt;
+      90             :   }
+      91           4 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      92           4 :     return std::nullopt;
+      93             :   }
+      94           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+      95           2 :     return std::nullopt;
+      96             :   }
+      97             : };
+      98             : 
+      99             : //}
+     100             : 
+     101             : }  // namespace common
+     102             : 
+     103             : }  // namespace mrs_uav_controllers
+     104             : 
+     105             : #endif  // MRS_UAV_CONTROLLERS_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.gcov.overview.html b/mrs_uav_controllers/include/common.h.gcov.overview.html new file mode 100644 index 0000000000..42538a882f --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.overview.html @@ -0,0 +1,47 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/include/common.h.gcov.png b/mrs_uav_controllers/include/common.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..042ffaa6e8f2e8a56ad432456b5e16b86c9a1a42 GIT binary patch literal 436 zcmV;l0ZaagP)#^9ia;?Oh<);&1n;{`V)#)grKCzA%7BS577Yq& z5+&x4JP}o7{Ly42B6iV-v91#DuqLc}xHa5d_Yk2#;Y9#97rdz8W<^)%tNpgB`AL_}T^7{lT~x##yVi&>&{wCSk3s5OGCw1ZkFV z%0%z|G)=E*g+Iqkd^4{jB8)X_XUth_U0K_;igU^)%HNq^#npTNew-p^-I{gioHPB7 z?x}<-bgj~XSncp{k4M(L`AF-qxc+bb*R1zZJn>jR))%aAxLyzcjgutzGk?4ydjY_) e#FpJEikcq_FD>HrpF4K|0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-25 05:42:57Functions:1414100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
pid.hpp +
100.0%
+ 		100.0 %33 / 33100.0 %5 / 5
<unnamed>100.0 %33 / 33100.0 %5 / 5
common.h +
96.0%96.0%
+ 		96.0 %24 / 25100.0 %9 / 9
<unnamed>96.0 %24 / 25100.0 %9 / 9
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/index-detail-sort-l.html b/mrs_uav_controllers/include/index-detail-sort-l.html new file mode 100644 index 0000000000..37140f0cc5 --- /dev/null +++ b/mrs_uav_controllers/include/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-25 05:42:57Functions:1414100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
96.0%96.0%
+ 		96.0 %24 / 25100.0 %9 / 9
<unnamed>96.0 %24 / 25100.0 %9 / 9
pid.hpp +
100.0%
+ 		100.0 %33 / 33100.0 %5 / 5
<unnamed>100.0 %33 / 33100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/index-detail.html b/mrs_uav_controllers/include/index-detail.html new file mode 100644 index 0000000000..4bed4153d1 --- /dev/null +++ b/mrs_uav_controllers/include/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-25 05:42:57Functions:1414100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
96.0%96.0%
+ 		96.0 %24 / 25100.0 %9 / 9
<unnamed>96.0 %24 / 25100.0 %9 / 9
pid.hpp +
100.0%
+ 		100.0 %33 / 33100.0 %5 / 5
<unnamed>100.0 %33 / 33100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/index-sort-f.html b/mrs_uav_controllers/include/index-sort-f.html new file mode 100644 index 0000000000..e8e899b9f4 --- /dev/null +++ b/mrs_uav_controllers/include/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-25 05:42:57Functions:1414100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
pid.hpp +
100.0%
+ 		100.0 %33 / 33100.0 %5 / 5
common.h +
96.0%96.0%
+ 		96.0 %24 / 25100.0 %9 / 9
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/index-sort-l.html b/mrs_uav_controllers/include/index-sort-l.html new file mode 100644 index 0000000000..13c2217fbe --- /dev/null +++ b/mrs_uav_controllers/include/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-25 05:42:57Functions:1414100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
96.0%96.0%
+ 		96.0 %24 / 25100.0 %9 / 9
pid.hpp +
100.0%
+ 		100.0 %33 / 33100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/index.html b/mrs_uav_controllers/include/index.html new file mode 100644 index 0000000000..1dac5df0bd --- /dev/null +++ b/mrs_uav_controllers/include/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-25 05:42:57Functions:1414100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
96.0%96.0%
+ 		96.0 %24 / 25100.0 %9 / 9
pid.hpp +
100.0%
+ 		100.0 %33 / 33100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/pid.hpp.func-sort-c.html b/mrs_uav_controllers/include/pid.hpp.func-sort-c.html new file mode 100644 index 0000000000..863f47ac40 --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3333100.0 %
Date:2024-04-25 05:42:57Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::reset()1128
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)1128
mrs_uav_controllers::PIDController::PIDController()1128
mrs_uav_controllers::PIDController::setSaturation(double)14190
mrs_uav_controllers::PIDController::update(double const&, double const&)14190
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/pid.hpp.func.html b/mrs_uav_controllers/include/pid.hpp.func.html new file mode 100644 index 0000000000..e976954947 --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3333100.0 %
Date:2024-04-25 05:42:57Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::setSaturation(double)14190
mrs_uav_controllers::PIDController::reset()1128
mrs_uav_controllers::PIDController::update(double const&, double const&)14190
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)1128
mrs_uav_controllers::PIDController::PIDController()1128
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/pid.hpp.gcov.frameset.html b/mrs_uav_controllers/include/pid.hpp.gcov.frameset.html new file mode 100644 index 0000000000..2641a21502 --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/include/pid.hpp.gcov.html b/mrs_uav_controllers/include/pid.hpp.gcov.html new file mode 100644 index 0000000000..794d4a456b --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.gcov.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3333100.0 %
Date:2024-04-25 05:42:57Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef PID_H
+       2             : #define PID_H
+       3             : 
+       4             : #include <math.h>
+       5             : 
+       6             : namespace mrs_uav_controllers
+       7             : {
+       8             : 
+       9             : class PIDController {
+      10             : 
+      11             : private:
+      12             :   // | ----------------------- parameters ----------------------- |
+      13             : 
+      14             :   // gains
+      15             :   double _kp_ = 0;  // proportional gain
+      16             :   double _kd_ = 0;  // derivative gain
+      17             :   double _ki_ = 0;  // integral gain
+      18             : 
+      19             :   // we remember the last control error, to calculate the difference
+      20             :   double last_error_ = 0;
+      21             :   double integral_   = 0;
+      22             : 
+      23             :   double saturation = -1;
+      24             :   double antiwindup = -1;
+      25             : 
+      26             : public:
+      27             :   PIDController();
+      28             : 
+      29             :   void setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup);
+      30             : 
+      31             :   void setSaturation(const double saturation = -1);
+      32             : 
+      33             :   void reset(void);
+      34             : 
+      35             :   double update(const double &error, const double &dt);
+      36             : };
+      37             : 
+      38             : // --------------------------------------------------------------
+      39             : // |                       implementation                       |
+      40             : // --------------------------------------------------------------
+      41             : 
+      42        1128 : PIDController::PIDController() {
+      43             : 
+      44        1128 :   this->reset();
+      45        1128 : }
+      46             : 
+      47        1128 : void PIDController::setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup) {
+      48             : 
+      49        1128 :   this->_kp_       = kp;
+      50        1128 :   this->_kd_       = kd;
+      51        1128 :   this->_ki_       = ki;
+      52        1128 :   this->saturation = saturation;
+      53        1128 :   this->antiwindup = antiwindup;
+      54        1128 : }
+      55             : 
+      56       14190 : void PIDController::setSaturation(const double saturation) {
+      57             : 
+      58       14190 :   this->saturation = saturation;
+      59       14190 : }
+      60             : 
+      61        1128 : void PIDController::reset(void) {
+      62             : 
+      63        1128 :   this->last_error_ = 0;
+      64        1128 :   this->integral_   = 0;
+      65        1128 : }
+      66             : 
+      67       14190 : double PIDController::update(const double &error, const double &dt) {
+      68             : 
+      69             :   // calculate the control error difference
+      70       14190 :   double difference = (error - last_error_) / dt;
+      71       14190 :   last_error_       = error;
+      72             : 
+      73       14190 :   double p_component = _kp_ * error;       // proportional feedback
+      74       14190 :   double d_component = _kd_ * difference;  // derivative feedback
+      75       14190 :   double i_component = _ki_ * integral_;   // derivative feedback
+      76             : 
+      77       14190 :   double sum = p_component + d_component + i_component;
+      78             : 
+      79       14190 :   if (saturation > 0) {
+      80       14190 :     if (sum >= saturation) {
+      81         129 :       sum = saturation;
+      82       14061 :     } else if (sum <= -saturation) {
+      83         166 :       sum = -saturation;
+      84             :     }
+      85             :   }
+      86             : 
+      87       14190 :   if (antiwindup > 0) {
+      88       14190 :     if (std::abs(sum) < antiwindup) {
+      89             :       // add to the integral
+      90       13492 :       integral_ += error * dt;
+      91             :     }
+      92             :   }
+      93             : 
+      94             :   // return the summ of the components
+      95       14190 :   return sum;
+      96             : }
+      97             : 
+      98             : }  // namespace mrs_uav_controllers
+      99             : 
+     100             : #endif  // PID_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/pid.hpp.gcov.overview.html b/mrs_uav_controllers/include/pid.hpp.gcov.overview.html new file mode 100644 index 0000000000..1e2dbca381 --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.gcov.overview.html @@ -0,0 +1,45 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/include/pid.hpp.gcov.png b/mrs_uav_controllers/include/pid.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..210752eb0f7abbe4f604ca452c63cabd54fcbebc GIT binary patch literal 519 zcmV+i0{H!jP)i;6ZDy3l1+EM97q1wFfy?;^t%ULDl)5$x zudZxs9|wgN#&S+{%-+D_U+^5KcAHUcKec%`Ff)4a1ZJuwkkpJ?V2Hm?hhj?cyI$-j z6IR|&6Z#l1hU=$#>Vf|txG3H9(th`RKN%W%c##Z|uGZuhz)xh|)X=9I-kXLt8S|8a zV%YX4lM~-WcfrHDO)|?9Z_{+J>{Z4~pHXJ@bSWS)bb#Sm1{SzJ&~<@(G5U+N_@Nr8 zXB3uUa|&Kps($zM!m literal 0 HcmV?d00001 diff --git a/mrs_uav_controllers/src/common.cpp.func-sort-c.html b/mrs_uav_controllers/src/common.cpp.func-sort-c.html new file mode 100644 index 0000000000..fd8884b5ef --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-04-25 05:42:57Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::common::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)227
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3752
mrs_uav_controllers::common::attitudeRateController(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)7507
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)10123
mrs_uav_controllers::common::so3transform(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, bool const&)83985
mrs_uav_controllers::common::sanitizeDesiredForce(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double const&, double const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)83985
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)85964
mrs_uav_controllers::common::attitudeController(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, bool const&)85964
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)90152
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/common.cpp.func.html b/mrs_uav_controllers/src/common.cpp.func.html new file mode 100644 index 0000000000..f297884113 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-04-25 05:42:57Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::common::so3transform(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, bool const&)83985
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3752
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)90152
mrs_uav_controllers::common::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)227
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)10123
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)85964
mrs_uav_controllers::common::attitudeController(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, bool const&)85964
mrs_uav_controllers::common::sanitizeDesiredForce(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double const&, double const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)83985
mrs_uav_controllers::common::attitudeRateController(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)7507
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/common.cpp.gcov.frameset.html b/mrs_uav_controllers/src/common.cpp.gcov.frameset.html new file mode 100644 index 0000000000..f24fe27b38 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/src/common.cpp.gcov.html b/mrs_uav_controllers/src/common.cpp.gcov.html new file mode 100644 index 0000000000..f788ee002b --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.gcov.html @@ -0,0 +1,471 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-04-25 05:42:57Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <common.h>
+       2             : 
+       3             : namespace mrs_uav_controllers
+       4             : {
+       5             : 
+       6             : namespace common
+       7             : {
+       8             : 
+       9             : /* orientationError() //{ */
+      10             : 
+      11       85964 : Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d& Rd) {
+      12             : 
+      13             :   // orientation error
+      14       85964 :   Eigen::Matrix3d R_error = 0.5 * (Rd.transpose() * R - R.transpose() * Rd);
+      15             : 
+      16             :   // vectorize the orientation error
+      17             :   // clang-format off
+      18       85964 :     Eigen::Vector3d R_error_vec;
+      19       85964 :     R_error_vec << (R_error(1, 2) - R_error(2, 1)) / 2.0,
+      20       85964 :                    (R_error(2, 0) - R_error(0, 2)) / 2.0,
+      21       85964 :                    (R_error(0, 1) - R_error(1, 0)) / 2.0;
+      22             :   // clang-format on
+      23             : 
+      24      171928 :   return R_error_vec;
+      25             : }
+      26             : 
+      27             : //}
+      28             : 
+      29             : /* sanitizeDesiredForce() //{ */
+      30             : 
+      31       83985 : std::optional<Eigen::Vector3d> sanitizeDesiredForce(const Eigen::Vector3d& input, const double& tilt_safety_limit, const double& tilt_saturation,
+      32             :                                                     const std::string& node_name) {
+      33             : 
+      34             :   // calculate the force in spherical coordinates
+      35       83985 :   double theta = acos(input(2));
+      36       83985 :   double phi   = atan2(input(1), input(0));
+      37             : 
+      38             :   // check for the failsafe limit
+      39       83985 :   if (!std::isfinite(theta)) {
+      40           0 :     ROS_ERROR("[%s]: sanitizeDesiredForce(): NaN detected in variable 'theta', returning empty command", node_name.c_str());
+      41           0 :     return {};
+      42             :   }
+      43             : 
+      44       83985 :   if (tilt_safety_limit > 1e-3 && std::abs(theta) > tilt_safety_limit) {
+      45             : 
+      46           0 :     ROS_ERROR("[%s]: the produced tilt angle (%.2f deg) would be over the failsafe limit (%.2f deg), returning null", node_name.c_str(), (180.0 / M_PI) * theta,
+      47             :               (180.0 / M_PI) * tilt_safety_limit);
+      48           0 :     ROS_ERROR_STREAM("[" << node_name << "]: f = [" << input.transpose() << "]");
+      49             : 
+      50           0 :     return {};
+      51             :   }
+      52             : 
+      53             :   // saturate the angle
+      54             : 
+      55       83985 :   if (tilt_saturation > 1e-3 && std::abs(theta) > tilt_saturation) {
+      56           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: tilt is being saturated, desired: %.2f deg, saturated %.2f deg", node_name.c_str(), (theta / M_PI) * 180.0,
+      57             :                       (tilt_saturation / M_PI) * 180.0);
+      58           0 :     theta = tilt_saturation;
+      59             :   }
+      60             : 
+      61             :   // reconstruct the force vector back out of the spherical coordinates
+      62       83985 :   Eigen::Vector3d output(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
+      63             : 
+      64       83985 :   return {output};
+      65             : }
+      66             : 
+      67             : //}
+      68             : 
+      69             : /* so3transform() //{ */
+      70             : 
+      71       83985 : Eigen::Matrix3d so3transform(const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading) {
+      72             : 
+      73       83985 :   Eigen::Vector3d body_z_normed = body_z.normalized();
+      74             : 
+      75       83985 :   Eigen::Matrix3d Rd;
+      76             : 
+      77       83985 :   if (preserve_heading) {
+      78             : 
+      79       24276 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Baca's method");
+      80             : 
+      81             :     // | ------------------------- body z ------------------------- |
+      82       24276 :     Rd.col(2) = body_z_normed;
+      83             : 
+      84             :     // | ------------------------- body x ------------------------- |
+      85             : 
+      86             :     // construct the oblique projection
+      87       24276 :     Eigen::Matrix3d projector_body_z_compl = (Eigen::Matrix3d::Identity(3, 3) - body_z_normed * body_z_normed.transpose());
+      88             : 
+      89             :     // create a basis of the body-z complement subspace
+      90       48552 :     Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+      91       24276 :     A.col(0)          = projector_body_z_compl.col(0);
+      92       24276 :     A.col(1)          = projector_body_z_compl.col(1);
+      93             : 
+      94             :     // create the basis of the projection null-space complement
+      95       48552 :     Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+      96       24276 :     B.col(0)          = Eigen::Vector3d(1, 0, 0);
+      97       24276 :     B.col(1)          = Eigen::Vector3d(0, 1, 0);
+      98             : 
+      99             :     // oblique projector to <range_basis>
+     100       48552 :     Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     101       48552 :     Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     102       24276 :     Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     103             : 
+     104       24276 :     Rd.col(0) = oblique_projector * heading;
+     105       24276 :     Rd.col(0).normalize();
+     106             : 
+     107             :     // | ------------------------- body y ------------------------- |
+     108             : 
+     109       24276 :     Rd.col(1) = Rd.col(2).cross(Rd.col(0));
+     110       24276 :     Rd.col(1).normalize();
+     111             : 
+     112             :   } else {
+     113             : 
+     114       59709 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Lee's method");
+     115             : 
+     116       59709 :     Rd.col(2) = body_z_normed;
+     117       59709 :     Rd.col(1) = Rd.col(2).cross(heading);
+     118       59709 :     Rd.col(1).normalize();
+     119       59709 :     Rd.col(0) = Rd.col(1).cross(Rd.col(2));
+     120       59709 :     Rd.col(0).normalize();
+     121             :   }
+     122             : 
+     123      167970 :   return Rd;
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* getLowestOutput() //{ */
+     129             : 
+     130       90152 : std::optional<CONTROL_OUTPUT> getLowestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     131             : 
+     132       90152 :   if (outputs.actuators) {
+     133        2359 :     return ACTUATORS_CMD;
+     134             :   }
+     135             : 
+     136       87793 :   if (outputs.control_group) {
+     137        2362 :     return CONTROL_GROUP;
+     138             :   }
+     139             : 
+     140       85431 :   if (outputs.attitude_rate) {
+     141       77057 :     return ATTITUDE_RATE;
+     142             :   }
+     143             : 
+     144        8374 :   if (outputs.attitude) {
+     145        2207 :     return ATTITUDE;
+     146             :   }
+     147             : 
+     148        6167 :   if (outputs.acceleration_hdg_rate) {
+     149         997 :     return ACCELERATION_HDG_RATE;
+     150             :   }
+     151             : 
+     152        5170 :   if (outputs.acceleration_hdg) {
+     153        1055 :     return ACCELERATION_HDG;
+     154             :   }
+     155             : 
+     156        4115 :   if (outputs.velocity_hdg_rate) {
+     157        2318 :     return VELOCITY_HDG_RATE;
+     158             :   }
+     159             : 
+     160        1797 :   if (outputs.velocity_hdg) {
+     161        1307 :     return VELOCITY_HDG;
+     162             :   }
+     163             : 
+     164         490 :   if (outputs.position) {
+     165         490 :     return POSITION;
+     166             :   }
+     167             : 
+     168           0 :   return {};
+     169             : }
+     170             : 
+     171             : //}
+     172             : 
+     173             : /* getHighestOutput() //{ */
+     174             : 
+     175       10123 : std::optional<CONTROL_OUTPUT> getHighestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     176             : 
+     177       10123 :   if (outputs.position) {
+     178           4 :     return POSITION;
+     179             :   }
+     180             : 
+     181       10119 :   if (outputs.velocity_hdg) {
+     182           7 :     return VELOCITY_HDG;
+     183             :   }
+     184             : 
+     185       10112 :   if (outputs.velocity_hdg_rate) {
+     186          11 :     return VELOCITY_HDG_RATE;
+     187             :   }
+     188             : 
+     189       10101 :   if (outputs.acceleration_hdg) {
+     190           7 :     return ACCELERATION_HDG;
+     191             :   }
+     192             : 
+     193       10094 :   if (outputs.acceleration_hdg_rate) {
+     194           6 :     return ACCELERATION_HDG_RATE;
+     195             :   }
+     196             : 
+     197       10088 :   if (outputs.attitude) {
+     198        5821 :     return ATTITUDE;
+     199             :   }
+     200             : 
+     201        4267 :   if (outputs.attitude_rate) {
+     202        1433 :     return ATTITUDE_RATE;
+     203             :   }
+     204             : 
+     205        2834 :   if (outputs.control_group) {
+     206        1417 :     return CONTROL_GROUP;
+     207             :   }
+     208             : 
+     209        1417 :   if (outputs.actuators) {
+     210        1417 :     return ACTUATORS_CMD;
+     211             :   }
+     212             : 
+     213           0 :   return {};
+     214             : }
+     215             : 
+     216             : //}
+     217             : 
+     218             : /* extractThrottle() //{ */
+     219             : 
+     220         227 : std::optional<double> extractThrottle(const mrs_uav_managers::Controller::ControlOutput& control_output) {
+     221             : 
+     222         227 :   if (!control_output.control_output) {
+     223         123 :     return {};
+     224             :   }
+     225             : 
+     226         208 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* attitudeController() //{ */
+     232             : 
+     233       85964 : std::optional<mrs_msgs::HwApiAttitudeRateCmd> attitudeController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeCmd& reference,
+     234             :                                                                  const Eigen::Vector3d& ff_rate, const Eigen::Vector3d& rate_saturation,
+     235             :                                                                  const Eigen::Vector3d& gains, const bool& parasitic_heading_rate_compensation) {
+     236             : 
+     237       85964 :   Eigen::Matrix3d R  = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     238       85964 :   Eigen::Matrix3d Rd = mrs_lib::AttitudeConverter(reference.orientation);
+     239             : 
+     240             :   // calculate the orientation error
+     241       85964 :   Eigen::Vector3d E = common::orientationError(R, Rd);
+     242             : 
+     243       85964 :   Eigen::Vector3d rate_feedback = gains.array() * E.array() + ff_rate.array();
+     244             : 
+     245             :   // | ----------- parasitic heading rate compensation ---------- |
+     246             : 
+     247       85964 :   if (parasitic_heading_rate_compensation) {
+     248             : 
+     249       23127 :     ROS_DEBUG_THROTTLE(1.0, "[AttitudeController]: parasitic heading rate compensation enabled");
+     250             : 
+     251             :     // compensate for the parasitic heading rate created by the desired pitch and roll rate
+     252       23127 :     Eigen::Vector3d rp_heading_rate_compensation(0, 0, 0);
+     253             : 
+     254       23127 :     Eigen::Vector3d q_feedback_yawless = rate_feedback;
+     255       23127 :     q_feedback_yawless(2)              = 0;  // nullyfy the effect of the original yaw feedback
+     256             : 
+     257       23127 :     double parasitic_heading_rate = 0;
+     258             : 
+     259             :     try {
+     260       23127 :       parasitic_heading_rate = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeadingRate(q_feedback_yawless);
+     261             :     }
+     262           0 :     catch (...) {
+     263           0 :       ROS_ERROR("[AttitudeController]: exception caught while calculating the parasitic heading rate!");
+     264             :     }
+     265             : 
+     266             :     try {
+     267       23127 :       rp_heading_rate_compensation(2) = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYawRateIntrinsic(-parasitic_heading_rate);
+     268             :     }
+     269           0 :     catch (...) {
+     270           0 :       ROS_ERROR("[AttitudeController]: exception caught while calculating the parasitic heading rate compensation!");
+     271             :     }
+     272             : 
+     273       23127 :     rate_feedback += rp_heading_rate_compensation;
+     274             :   }
+     275             : 
+     276             :   // | --------------- saturate the attitude rate --------------- |
+     277             : 
+     278       85964 :   if (rate_feedback(0) > rate_saturation(0)) {
+     279           0 :     rate_feedback(0) = rate_saturation(0);
+     280       85964 :   } else if (rate_feedback(0) < -rate_saturation(0)) {
+     281           0 :     rate_feedback(0) = -rate_saturation(0);
+     282             :   }
+     283             : 
+     284       85964 :   if (rate_feedback(1) > rate_saturation(1)) {
+     285           0 :     rate_feedback(1) = rate_saturation(1);
+     286       85964 :   } else if (rate_feedback(1) < -rate_saturation(1)) {
+     287           0 :     rate_feedback(1) = -rate_saturation(1);
+     288             :   }
+     289             : 
+     290       85964 :   if (rate_feedback(2) > rate_saturation(2)) {
+     291           0 :     rate_feedback(2) = rate_saturation(2);
+     292       85964 :   } else if (rate_feedback(2) < -rate_saturation(2)) {
+     293           0 :     rate_feedback(2) = -rate_saturation(2);
+     294             :   }
+     295             : 
+     296             :   // | ------------ fill in the attitude rate command ----------- |
+     297             : 
+     298       85964 :   mrs_msgs::HwApiAttitudeRateCmd cmd;
+     299             : 
+     300       85964 :   cmd.stamp = ros::Time::now();
+     301             : 
+     302       85964 :   cmd.body_rate.x = rate_feedback(0);
+     303       85964 :   cmd.body_rate.y = rate_feedback(1);
+     304       85964 :   cmd.body_rate.z = rate_feedback(2);
+     305             : 
+     306       85964 :   cmd.throttle = reference.throttle;
+     307             : 
+     308      171928 :   return cmd;
+     309             : }
+     310             : 
+     311             : //}
+     312             : 
+     313             : /* attitudeRateController() //{ */
+     314             : 
+     315        7507 : std::optional<mrs_msgs::HwApiControlGroupCmd> attitudeRateController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeRateCmd& reference,
+     316             :                                                                      const Eigen::Vector3d& gains) {
+     317             : 
+     318        7507 :   Eigen::Vector3d des_rate(reference.body_rate.x, reference.body_rate.y, reference.body_rate.z);
+     319        7507 :   Eigen::Vector3d cur_rate(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     320             : 
+     321        7507 :   Eigen::Vector3d ctrl_group_action = (des_rate - cur_rate).array() * gains.array();
+     322             : 
+     323        7507 :   mrs_msgs::HwApiControlGroupCmd cmd;
+     324             : 
+     325        7507 :   cmd.stamp = ros::Time::now();
+     326             : 
+     327        7507 :   cmd.throttle = reference.throttle;
+     328             : 
+     329        7507 :   cmd.roll  = ctrl_group_action(0);
+     330        7507 :   cmd.pitch = ctrl_group_action(1);
+     331        7507 :   cmd.yaw   = ctrl_group_action(2);
+     332             : 
+     333       15014 :   return {cmd};
+     334             : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* actuatorMixer() //{ */
+     339             : 
+     340        3752 : mrs_msgs::HwApiActuatorCmd actuatorMixer(const mrs_msgs::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer) {
+     341             : 
+     342        3752 :   Eigen::Vector4d ctrl_group(ctrl_group_cmd.roll, ctrl_group_cmd.pitch, ctrl_group_cmd.yaw, ctrl_group_cmd.throttle);
+     343             : 
+     344        7504 :   Eigen::VectorXd motors = mixer * ctrl_group;
+     345             : 
+     346        3752 :   double min = motors.minCoeff();
+     347             : 
+     348        3752 :   if (min < 0.0) {
+     349           0 :     motors.array() += abs(min);
+     350             :   }
+     351             : 
+     352        3752 :   double max = motors.maxCoeff();
+     353             : 
+     354        3752 :   if (max > 1.0) {
+     355             : 
+     356           0 :     if (ctrl_group_cmd.throttle > 1e-2) {
+     357             : 
+     358             :       // scale down roll/pitch/yaw actions to maintain desired throttle
+     359           0 :       for (int i = 0; i < 3; i++) {
+     360           0 :         ctrl_group(i) = ctrl_group(i) / (motors.mean() / ctrl_group_cmd.throttle);
+     361             :       }
+     362             : 
+     363           0 :       motors = mixer * ctrl_group;
+     364             : 
+     365             :     } else {
+     366           0 :       motors /= max;
+     367             :     }
+     368             :   }
+     369             : 
+     370             :   // | --------------------- fill in the msg -------------------- |
+     371             : 
+     372        3752 :   mrs_msgs::HwApiActuatorCmd actuator_msg;
+     373             : 
+     374        3752 :   actuator_msg.stamp = ros::Time::now();
+     375             : 
+     376       18760 :   for (int i = 0; i < motors.size(); i++) {
+     377       15008 :     actuator_msg.motors.push_back(motors(i));
+     378             :   }
+     379             : 
+     380        7504 :   return actuator_msg;
+     381             : }
+     382             : 
+     383             : //}
+     384             : 
+     385             : }  // namespace common
+     386             : 
+     387             : }  // namespace mrs_uav_controllers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/common.cpp.gcov.overview.html b/mrs_uav_controllers/src/common.cpp.gcov.overview.html new file mode 100644 index 0000000000..dd0d0db077 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.gcov.overview.html @@ -0,0 +1,117 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/common.cpp.gcov.png b/mrs_uav_controllers/src/common.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ae7a39fe11d1881d88a0dc8901f5aabbb7614895 GIT binary patch literal 1368 zcmV-e1*iInP)oiq$s z=NH2Am0;OgEB5CBD62zU z{0~B*^^;g{Gnu2ZEX$e<>)sE0Gw^o&IfD=AUo8*QLFz)L2chU0?~CiP#6q@!xE%yCid_+&Kx1$X`Ja*nN?~tAa88? zKxc45lH{g&hp4A`P%0=Wl0@2v;*`|P257*h)lzi5P09)vF~-ogLHF{kqn$e_BWA63 zT|ZzqrfK_Pm;^p)TdPy%vB_^ddlL%xod}6F9ZySpBI5RJbHNG^7bEJFc_eM9K18AS zlz}qTG>9r?&@oJ7mQ9@k|2en>0O4Qr9>_{2d{;rV02Nei0?e0~vk3mHFkQt?X%&Hgh@e!z5aZl>AF1HmZ-6fatSs6np!&6Xc} zl~Ps9t}XOBAwsy zCno%9ht(gGkktU1U^rzS?3-V2o}jedXND`JIX(+@LWrcETCVa3s@*(hGUJ zX|OEox?KlhI$&d+#G`*{E$i=nh&oZ;h0p!H6po_8Ji=3m$a-zvr$E-y`SleyI!TE% z;=y6EuKZ?Y&u=@QDHT#Az85o-%Zvb!mS>^5S1}ZeGfWz|cBi6E@eaeVNp}VNQ$$a# a + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()34
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::failsafe_controller::FailsafeController::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)372
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()972
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9723
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)127132
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.func.html b/mrs_uav_controllers/src/failsafe_controller.cpp.func.html new file mode 100644 index 0000000000..642053aa20 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()34
mrs_uav_controllers::failsafe_controller::FailsafeController::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9723
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)372
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)127132
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()972
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html new file mode 100644 index 0000000000..482a0a3bc5 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html new file mode 100644 index 0000000000..6580f615df --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html @@ -0,0 +1,635 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <common.h>
+       7             : 
+       8             : #include <mrs_uav_managers/controller.h>
+       9             : 
+      10             : #include <mrs_lib/profiler.h>
+      11             : #include <mrs_lib/attitude_converter.h>
+      12             : #include <mrs_lib/mutex.h>
+      13             : #include <mrs_lib/subscribe_handler.h>
+      14             : 
+      15             : //}
+      16             : 
+      17             : namespace mrs_uav_controllers
+      18             : {
+      19             : 
+      20             : namespace failsafe_controller
+      21             : {
+      22             : 
+      23             : /* class FailsafeController //{ */
+      24             : 
+      25             : class FailsafeController : public mrs_uav_managers::Controller {
+      26             : 
+      27             : public:
+      28             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      29             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      30             : 
+      31             :   bool activate(const ControlOutput &last_control_output);
+      32             :   void deactivate(void);
+      33             : 
+      34             :   void updateInactive(const mrs_msgs::UavState &uav_state, const std::optional<mrs_msgs::TrackerCommand> &tracker_command);
+      35             : 
+      36             :   ControlOutput updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+      37             : 
+      38             :   const mrs_msgs::ControllerStatus getStatus();
+      39             : 
+      40             :   void switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      41             : 
+      42             :   void resetDisturbanceEstimators(void);
+      43             : 
+      44             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      45             : 
+      46             :   double getHeadingSafely(const geometry_msgs::Quaternion &quaternion);
+      47             : 
+      48             : private:
+      49             :   ros::NodeHandle nh_;
+      50             : 
+      51             :   bool is_initialized_ = false;
+      52             :   bool is_active_      = false;
+      53             : 
+      54             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      55             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      56             : 
+      57             :   // | ----------------------- parameters ----------------------- |
+      58             : 
+      59             :   double _descend_speed_;
+      60             :   double _descend_acceleration_;
+      61             : 
+      62             :   double _kq_;
+      63             :   double _kw_;
+      64             : 
+      65             :   // | ------------------- remember uav state ------------------- |
+      66             : 
+      67             :   mrs_msgs::UavState uav_state_;
+      68             :   std::mutex         mutex_uav_state_;
+      69             : 
+      70             :   // | --------------------- throttle control --------------------- |
+      71             : 
+      72             :   double _uav_mass_;
+      73             :   double uav_mass_difference_;
+      74             : 
+      75             :   double hover_throttle_;
+      76             : 
+      77             :   double _throttle_decrease_rate_;
+      78             :   double _initial_throttle_percentage_;
+      79             : 
+      80             :   // | ----------------------- yaw control ---------------------- |
+      81             : 
+      82             :   double heading_setpoint_;
+      83             : 
+      84             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_hw_api_orientation_;
+      85             : 
+      86             :   // | ------------------ activation and output ----------------- |
+      87             : 
+      88             :   ControlOutput last_control_output_;
+      89             :   ControlOutput activation_control_output_;
+      90             : 
+      91             :   ros::Time         last_update_time_;
+      92             :   std::atomic<bool> first_iteration_ = true;
+      93             : 
+      94             :   // | ------------------------ profiler ------------------------ |
+      95             : 
+      96             :   mrs_lib::Profiler profiler_;
+      97             :   bool              _profiler_enabled_ = false;
+      98             : 
+      99             :   // | ----------------------- constraints ---------------------- |
+     100             : 
+     101             :   mrs_msgs::DynamicsConstraints constraints_;
+     102             :   std::mutex                    mutex_constraints_;
+     103             : };
+     104             : 
+     105             : //}
+     106             : 
+     107             : // --------------------------------------------------------------
+     108             : // |                   controller's interface                   |
+     109             : // --------------------------------------------------------------
+     110             : 
+     111             : /* initialize() //{ */
+     112             : 
+     113          94 : bool FailsafeController::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     114             :                                     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     115             : 
+     116          94 :   nh_ = nh;
+     117             : 
+     118          94 :   common_handlers_  = common_handlers;
+     119          94 :   private_handlers_ = private_handlers;
+     120             : 
+     121          94 :   _uav_mass_ = common_handlers->getMass();
+     122             : 
+     123          94 :   ros::Time::waitForValid();
+     124             : 
+     125             :   // | ---------- loading params using the parent's nh ---------- |
+     126             : 
+     127         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     128             : 
+     129          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     130             : 
+     131          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     132           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     133           0 :     return false;
+     134             :   }
+     135             : 
+     136             :   // | -------------------- loading my params ------------------- |
+     137             : 
+     138          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/failsafe_controller.yaml");
+     139          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/failsafe_controller.yaml");
+     140             : 
+     141         188 :   const std::string yaml_namespace = "mrs_uav_controllers/failsafe_controller/";
+     142             : 
+     143          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/throttle_decrease_rate", _throttle_decrease_rate_);
+     144          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/initial_throttle_percentage", _initial_throttle_percentage_);
+     145             : 
+     146          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "attitude_controller/gains/kp", _kq_);
+     147             : 
+     148          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "rate_controller/gains/kp", _kw_);
+     149             : 
+     150          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "velocity_output/descend_speed", _descend_speed_);
+     151          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "acceleration_output/descend_acceleration", _descend_acceleration_);
+     152             : 
+     153          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     154           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     155           0 :     return false;
+     156             :   }
+     157             : 
+     158          94 :   _descend_speed_        = std::abs(_descend_speed_);
+     159          94 :   _descend_acceleration_ = std::abs(_descend_acceleration_);
+     160             : 
+     161          94 :   uav_mass_difference_ = 0;
+     162             : 
+     163             :   // | ----------------------- subscribers ---------------------- |
+     164             : 
+     165          94 :   mrs_lib::SubscribeHandlerOptions shopts;
+     166          94 :   shopts.nh                 = nh_;
+     167          94 :   shopts.node_name          = "FailsafeController";
+     168          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     169          94 :   shopts.threadsafe         = true;
+     170          94 :   shopts.autostart          = true;
+     171          94 :   shopts.queue_size         = 10;
+     172          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     173             : 
+     174          94 :   sh_hw_api_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + common_handlers->uav_name + "/" + "hw_api/orientation");
+     175             : 
+     176             :   // | ----------- calculate the default hover throttle ----------- |
+     177             : 
+     178          94 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     179             : 
+     180             :   // | ------------------------ profiler ------------------------ |
+     181             : 
+     182          94 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "FailsafeController", _profiler_enabled_);
+     183             : 
+     184             :   // | ----------------------- finish init ---------------------- |
+     185             : 
+     186          94 :   ROS_INFO("[FailsafeController]: initialized");
+     187             : 
+     188          94 :   is_initialized_ = true;
+     189             : 
+     190          94 :   return true;
+     191             : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* activate() //{ */
+     196             : 
+     197           8 : bool FailsafeController::activate(const ControlOutput &last_control_output) {
+     198             : 
+     199          16 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     200             : 
+     201           8 :   if (!last_control_output.control_output) {
+     202             : 
+     203           0 :     ROS_WARN("[FailsafeController]: activated without getting the last controller's command");
+     204             : 
+     205           0 :     return false;
+     206             : 
+     207             :   } else {
+     208             : 
+     209             :     // | -------------- calculate the initial heading ------------- |
+     210             : 
+     211           8 :     if (sh_hw_api_orientation_.getMsg()) {
+     212             : 
+     213          16 :       auto hw_api_orientation = sh_hw_api_orientation_.getMsg();
+     214             : 
+     215           8 :       heading_setpoint_ = getHeadingSafely(hw_api_orientation->quaternion);
+     216             : 
+     217           8 :       ROS_INFO("[FailsafeController]: activated with heading = %.2f rad", heading_setpoint_);
+     218             : 
+     219             :     } else {
+     220             : 
+     221           0 :       ROS_ERROR("[FailsafeController]: missing orientation from HW API, activated with heading = 0 rad");
+     222             : 
+     223           0 :       heading_setpoint_ = 0;
+     224             :     }
+     225             : 
+     226           8 :     activation_control_output_ = last_control_output;
+     227             : 
+     228           8 :     if (last_control_output.diagnostics.mass_estimator) {
+     229           8 :       uav_mass_difference_ = last_control_output.diagnostics.mass_difference;
+     230             :     } else {
+     231           0 :       uav_mass_difference_ = 0;
+     232             :     }
+     233             : 
+     234           8 :     activation_control_output_.diagnostics.controller_enforcing_constraints = false;
+     235             : 
+     236           8 :     hover_throttle_ = _initial_throttle_percentage_ * mrs_lib::quadratic_throttle_model::forceToThrottle(
+     237           8 :                                                           common_handlers_->throttle_model, (_uav_mass_ + uav_mass_difference_) * common_handlers_->g);
+     238             : 
+     239           8 :     ROS_INFO("[FailsafeController]: activated with uav_mass_difference %.2f kg.", uav_mass_difference_);
+     240             :   }
+     241             : 
+     242           8 :   first_iteration_ = true;
+     243             : 
+     244           8 :   is_active_ = true;
+     245             : 
+     246           8 :   return true;
+     247             : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* deactivate() //{ */
+     252             : 
+     253          34 : void FailsafeController::deactivate(void) {
+     254             : 
+     255          34 :   is_active_           = false;
+     256          34 :   first_iteration_     = false;
+     257          34 :   uav_mass_difference_ = 0;
+     258             : 
+     259          34 :   ROS_INFO("[FailsafeController]: deactivated");
+     260          34 : }
+     261             : 
+     262             : //}
+     263             : 
+     264             : /* updateInactive() //{ */
+     265             : 
+     266      127132 : void FailsafeController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     267             : 
+     268      127132 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     269             : 
+     270      127132 :   last_update_time_ = ros::Time::now();
+     271             : 
+     272      127132 :   first_iteration_ = false;
+     273      127132 : }
+     274             : 
+     275             : //}
+     276             : 
+     277             : /* //{ updateWhenAcctive() */
+     278             : 
+     279        9723 : FailsafeController::ControlOutput FailsafeController::updateActive(const mrs_msgs::UavState &                       uav_state,
+     280             :                                                                    [[maybe_unused]] const mrs_msgs::TrackerCommand &tracker_command) {
+     281             : 
+     282       29169 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     283       29169 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("FailsafeController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     284             : 
+     285             :   {
+     286       19446 :     std::scoped_lock lock(mutex_uav_state_);
+     287             : 
+     288        9723 :     uav_state_ = uav_state;
+     289             :   }
+     290             : 
+     291        9723 :   if (!is_active_) {
+     292           0 :     return ControlOutput();
+     293             :   }
+     294             : 
+     295        9723 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     296             : 
+     297             :   // | -------------------- calculate the dt -------------------- |
+     298             : 
+     299             :   double dt;
+     300             : 
+     301        9723 :   if (first_iteration_) {
+     302           8 :     dt               = 0.01;
+     303           8 :     first_iteration_ = false;
+     304             :   } else {
+     305        9715 :     dt = (ros::Time::now() - last_update_time_).toSec();
+     306             :   }
+     307             : 
+     308        9723 :   last_update_time_ = ros::Time::now();
+     309             : 
+     310        9723 :   hover_throttle_ -= _throttle_decrease_rate_ * dt;
+     311             : 
+     312        9723 :   if (!std::isfinite(hover_throttle_)) {
+     313           0 :     hover_throttle_ = 0;
+     314           0 :     ROS_ERROR("[FailsafeController]: NaN detected in variable 'hover_throttle', setting it to 0 and returning!!!");
+     315        9723 :   } else if (hover_throttle_ > 1.0) {
+     316           0 :     hover_throttle_ = 1.0;
+     317        9723 :   } else if (hover_throttle_ < 0.0) {
+     318           0 :     hover_throttle_ = 0.0;
+     319             :   }
+     320             : 
+     321             :   // | --------------- prepare the control output --------------- |
+     322             : 
+     323       19446 :   FailsafeController::ControlOutput control_output;
+     324             : 
+     325        9723 :   control_output.diagnostics.controller = "FailsafeController";
+     326             : 
+     327        9723 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     328             : 
+     329        9723 :   if (!highest_modality) {
+     330           0 :     ROS_ERROR_THROTTLE(1.0, "[FailsafeController]: output modalities are empty! This error should never appear.");
+     331           0 :     return control_output;
+     332             :   }
+     333             : 
+     334        9723 :   control_output.diagnostics.controller = "FailsafeController";
+     335             : 
+     336             :   // --------------------------------------------------------------
+     337             :   // |                       position output                      |
+     338             :   // --------------------------------------------------------------
+     339             : 
+     340        9723 :   if (highest_modality.value() == common::POSITION) {
+     341           0 :     ROS_INFO_THROTTLE(1.0, "[FailsafeController]: returning empty command, because we are at the position modality");
+     342           0 :     return control_output;
+     343             :   }
+     344             : 
+     345             :   // --------------------------------------------------------------
+     346             :   // |                       velocity output                      |
+     347             :   // --------------------------------------------------------------
+     348             : 
+     349        9723 :   if (highest_modality.value() == common::VELOCITY_HDG) {
+     350             : 
+     351           0 :     mrs_msgs::HwApiVelocityHdgCmd vel_cmd;
+     352             : 
+     353           0 :     vel_cmd.header.stamp = ros::Time::now();
+     354             : 
+     355           0 :     vel_cmd.velocity.x = 0;
+     356           0 :     vel_cmd.velocity.y = 0;
+     357           0 :     vel_cmd.velocity.z = -_descend_speed_;
+     358           0 :     vel_cmd.heading    = heading_setpoint_;
+     359             : 
+     360           0 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning velocity+hdg output");
+     361           0 :     control_output.control_output = vel_cmd;
+     362           0 :     return control_output;
+     363             :   }
+     364             : 
+     365        9723 :   if (highest_modality.value() == common::VELOCITY_HDG_RATE) {
+     366             : 
+     367           0 :     mrs_msgs::HwApiVelocityHdgRateCmd vel_cmd;
+     368             : 
+     369           0 :     vel_cmd.header.stamp = ros::Time::now();
+     370             : 
+     371           0 :     vel_cmd.velocity.x   = 0;
+     372           0 :     vel_cmd.velocity.y   = 0;
+     373           0 :     vel_cmd.velocity.z   = -_descend_speed_;
+     374           0 :     vel_cmd.heading_rate = 0.0;
+     375             : 
+     376           0 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning velocity+hdg rate output");
+     377           0 :     control_output.control_output = vel_cmd;
+     378           0 :     return control_output;
+     379             :   }
+     380             : 
+     381             :   // --------------------------------------------------------------
+     382             :   // |                     acceleration output                    |
+     383             :   // --------------------------------------------------------------
+     384             : 
+     385        9723 :   if (highest_modality.value() == common::ACCELERATION_HDG) {
+     386             : 
+     387           0 :     mrs_msgs::HwApiAccelerationHdgCmd acc_cmd;
+     388             : 
+     389           0 :     acc_cmd.header.stamp = ros::Time::now();
+     390             : 
+     391           0 :     acc_cmd.acceleration.x = 0;
+     392           0 :     acc_cmd.acceleration.y = 0;
+     393           0 :     acc_cmd.acceleration.z = -_descend_acceleration_;
+     394           0 :     acc_cmd.heading        = heading_setpoint_;
+     395             : 
+     396           0 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning acceleration+hdg output");
+     397           0 :     control_output.control_output = acc_cmd;
+     398           0 :     return control_output;
+     399             :   }
+     400             : 
+     401        9723 :   if (highest_modality.value() == common::ACCELERATION_HDG_RATE) {
+     402             : 
+     403           0 :     mrs_msgs::HwApiAccelerationHdgRateCmd acc_cmd;
+     404             : 
+     405           0 :     acc_cmd.header.stamp = ros::Time::now();
+     406             : 
+     407           0 :     acc_cmd.acceleration.x = 0;
+     408           0 :     acc_cmd.acceleration.y = 0;
+     409           0 :     acc_cmd.acceleration.z = -_descend_acceleration_;
+     410           0 :     acc_cmd.heading_rate   = 0.0;
+     411             : 
+     412           0 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning acceleration+hdg rate output");
+     413           0 :     control_output.control_output = acc_cmd;
+     414           0 :     return control_output;
+     415             :   }
+     416             : 
+     417             :   // --------------------------------------------------------------
+     418             :   // |                       attitude output                      |
+     419             :   // --------------------------------------------------------------
+     420             : 
+     421        9723 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+     422             : 
+     423        9723 :   attitude_cmd.stamp       = ros::Time::now();
+     424        9723 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(0, 0, heading_setpoint_);
+     425        9723 :   attitude_cmd.throttle    = hover_throttle_;
+     426             : 
+     427        9723 :   if (highest_modality.value() == common::ATTITUDE) {
+     428        5537 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude output");
+     429        5537 :     control_output.control_output = attitude_cmd;
+     430        5537 :     return control_output;
+     431             :   }
+     432             : 
+     433             :   // --------------------------------------------------------------
+     434             :   // |                      attitude control                      |
+     435             :   // --------------------------------------------------------------
+     436             : 
+     437        4186 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+     438        4186 :   Eigen::Vector3d rate_ff(0, 0, 0);
+     439        4186 :   Eigen::Vector3d Kq(_kq_, _kq_, _kq_);
+     440             : 
+     441        4186 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, rate_ff, attitude_rate_saturation, Kq, false);
+     442             : 
+     443        4186 :   if (highest_modality.value() == common::ATTITUDE_RATE) {
+     444        1400 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude rate output");
+     445        1400 :     control_output.control_output = attitude_rate_command;
+     446        1400 :     return control_output;
+     447             :   }
+     448             : 
+     449             :   // --------------------------------------------------------------
+     450             :   // |                    attitude rate control                   |
+     451             :   // --------------------------------------------------------------
+     452             : 
+     453        2786 :   Eigen::Vector3d Kw = common_handlers_->detailed_model_params->inertia.diagonal() * _kw_;
+     454             : 
+     455        2786 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+     456             : 
+     457        2786 :   if (highest_modality.value() == common::CONTROL_GROUP) {
+     458        1393 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning control group output");
+     459        1393 :     control_output.control_output = control_group_command;
+     460        1393 :     return control_output;
+     461             :   }
+     462             : 
+     463             :   // --------------------------------------------------------------
+     464             :   // |                            mixer                           |
+     465             :   // --------------------------------------------------------------
+     466             : 
+     467        2786 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+     468             : 
+     469        1393 :   ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning actuators output");
+     470        1393 :   control_output.control_output = actuator_cmd;
+     471        1393 :   return control_output;
+     472             : }
+     473             : 
+     474             : //}
+     475             : 
+     476             : /* getStatus() //{ */
+     477             : 
+     478         972 : const mrs_msgs::ControllerStatus FailsafeController::getStatus() {
+     479             : 
+     480         972 :   mrs_msgs::ControllerStatus controller_status;
+     481             : 
+     482         972 :   controller_status.active = is_active_;
+     483             : 
+     484         972 :   return controller_status;
+     485             : }
+     486             : 
+     487             : //}
+     488             : 
+     489             : /* switchOdometrySource() //{ */
+     490             : 
+     491           0 : void FailsafeController::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     492           0 : }
+     493             : 
+     494             : //}
+     495             : 
+     496             : /* resetDisturbanceEstimators() //{ */
+     497             : 
+     498           0 : void FailsafeController::resetDisturbanceEstimators(void) {
+     499           0 : }
+     500             : 
+     501             : //}
+     502             : 
+     503             : /* setConstraints() //{ */
+     504             : 
+     505         372 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr FailsafeController::setConstraints([
+     506             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     507             : 
+     508         372 :   if (!is_initialized_) {
+     509           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     510             :   }
+     511             : 
+     512         372 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     513             : 
+     514         372 :   ROS_INFO("[FailsafeController]: updating constraints");
+     515             : 
+     516         744 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     517         372 :   res.success = true;
+     518         372 :   res.message = "constraints updated";
+     519             : 
+     520         372 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     521             : }
+     522             : 
+     523             : //}
+     524             : 
+     525             : /* getHeadingSafely() //{ */
+     526             : 
+     527           8 : double FailsafeController::getHeadingSafely(const geometry_msgs::Quaternion &quaternion) {
+     528             : 
+     529             :   try {
+     530           8 :     return mrs_lib::AttitudeConverter(quaternion).getHeading();
+     531             :   }
+     532           0 :   catch (...) {
+     533             :   }
+     534             : 
+     535             :   try {
+     536           0 :     return mrs_lib::AttitudeConverter(quaternion).getYaw();
+     537             :   }
+     538           0 :   catch (...) {
+     539             :   }
+     540             : 
+     541           0 :   return 0;
+     542             : }
+     543             : 
+     544             : //}
+     545             : 
+     546             : }  // namespace failsafe_controller
+     547             : 
+     548             : }  // namespace mrs_uav_controllers
+     549             : 
+     550             : #include <pluginlib/class_list_macros.h>
+     551          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_controllers::failsafe_controller::FailsafeController, mrs_uav_managers::Controller)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html new file mode 100644 index 0000000000..ba53a24ec7 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html @@ -0,0 +1,158 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.png b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..37f1ed5c8d31b8129d2f889eaed969e69d0777e4 GIT binary patch literal 1814 zcmV+x2kH2UP)Bulo_nv>+wE~{$ zG8^Ggl!8B*M>U9CZ-=pr;d?xbAor4ws!8hm8LY4Lgv)4^9f4B_W&Ow9LOCLZ zvXlU{kdKBjj7YE-C-{7nwUv`yDe_L-;_SR_GI(VBk_+(T7QE=Xyv zV6&M#!j&Zwld0C2h1oMsm^>{(YP@o(|Iqi=x_D+*uJ)-Anq^22@*52HqjOzLdR;Vl)C7V8Uz_RYCgGCzR0sA% z=mNq1T|WWwhOSEi;)D!>Pndq|k4SaJ^15e+69Q{B8S#L%-%!t5Kx-_BvfLtlhBJH0aX;=Fd1l*+wz}Vr zrSVI)Ql(TX`k6*#3ue3aUD}u6q;nZL4xE090~;_riJaf= znRJ*+hVLl&E(I#AFrrr$hWySDJgU6FBK|FM)L>Ic4d%lh`f=2T?Jtc1{<7CYmsx#z1(-mE!J??fEa5h)dIbnPeA?u-uu7y#BaSE8$vuUPuK3%*P$P-`tNUKX<%*e3umc z4PJFy$=p{um1g>B2`FJX2uf(JT)3d%^I+n!PW? zy^FrALL@4C)q-Uef#CiMf*IsG^Tg52QU7?HdZyr---f*+A5m0qI72Eh@_)j|7saz$ zK~hIF7^=)I9}LCE?Gb#ei{aspZ+P~T7>=U&pPe&T=QDMJaP$Si<2ez<`_7r;gGbC6 zI);ZR-onQ`BmZ?izD=|*LH?tBd`2FQW{CNgn+ohX6A)*3V=1UsbL|Cqdkg(&mvz$DKyPseLy z<#R_EUcl{H;#ain#t`$G1ll&|+-Sdl=L9keyYlopF_}lKJ8UZGm|tD5|6B-0)f4JrF}&9_ELlfKKS&O`nKve8+BM*eotrhk%;EFpT)d^q#T+#mQsAn<*71j< zKF7j9h($07*qoM6N<$ Ef(Ku2xBvhE literal 0 HcmV?d00001 diff --git a/mrs_uav_controllers/src/index-detail-sort-f.html b/mrs_uav_controllers/src/index-detail-sort-f.html new file mode 100644 index 0000000000..5ade009622 --- /dev/null +++ b/mrs_uav_controllers/src/index-detail-sort-f.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1751217480.5 %
Date:2024-04-25 05:42:57Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
<unnamed>67.5 %131 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %625 / 77588.2 %15 / 17
<unnamed>80.6 %625 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
<unnamed>81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-detail-sort-l.html b/mrs_uav_controllers/src/index-detail-sort-l.html new file mode 100644 index 0000000000..0049476bfa --- /dev/null +++ b/mrs_uav_controllers/src/index-detail-sort-l.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1751217480.5 %
Date:2024-04-25 05:42:57Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
<unnamed>67.5 %131 / 19481.8 %9 / 11
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %625 / 77588.2 %15 / 17
<unnamed>80.6 %625 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
<unnamed>81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-detail.html b/mrs_uav_controllers/src/index-detail.html new file mode 100644 index 0000000000..cf7d4dc8f6 --- /dev/null +++ b/mrs_uav_controllers/src/index-detail.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1751217480.5 %
Date:2024-04-25 05:42:57Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
<unnamed>67.5 %131 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
<unnamed>81.2 %727 / 89588.9 %16 / 18
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %625 / 77588.2 %15 / 17
<unnamed>80.6 %625 / 77588.2 %15 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-sort-f.html b/mrs_uav_controllers/src/index-sort-f.html new file mode 100644 index 0000000000..bcc3628748 --- /dev/null +++ b/mrs_uav_controllers/src/index-sort-f.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1751217480.5 %
Date:2024-04-25 05:42:57Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %625 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-sort-l.html b/mrs_uav_controllers/src/index-sort-l.html new file mode 100644 index 0000000000..cfba37025d --- /dev/null +++ b/mrs_uav_controllers/src/index-sort-l.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1751217480.5 %
Date:2024-04-25 05:42:57Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %625 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index.html b/mrs_uav_controllers/src/index.html new file mode 100644 index 0000000000..782803dd65 --- /dev/null +++ b/mrs_uav_controllers/src/index.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1751217480.5 %
Date:2024-04-25 05:42:57Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %625 / 77588.2 %15 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html b/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html new file mode 100644 index 0000000000..d907347b99 --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::midair_activation_controller::MidairActivationController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::midair_activation_controller::MidairActivationController::resetDisturbanceEstimators()0
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)18
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()89
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::midair_activation_controller::MidairActivationController::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()105
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)135
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)372
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)400
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)136455
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.func.html b/mrs_uav_controllers/src/midair_activation_controller.cpp.func.html new file mode 100644 index 0000000000..b8d2a6586b --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()89
mrs_uav_controllers::midair_activation_controller::MidairActivationController::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)400
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)372
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)136455
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)18
mrs_uav_controllers::midair_activation_controller::MidairActivationController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::midair_activation_controller::MidairActivationController::resetDisturbanceEstimators()0
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)135
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()105
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.frameset.html b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.frameset.html new file mode 100644 index 0000000000..0679c78cc0 --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html new file mode 100644 index 0000000000..f7408be54a --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html @@ -0,0 +1,518 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-04-25 05:42:57Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <common.h>
+       7             : 
+       8             : #include <mrs_uav_managers/controller.h>
+       9             : 
+      10             : #include <mrs_lib/profiler.h>
+      11             : #include <mrs_lib/attitude_converter.h>
+      12             : #include <mrs_lib/mutex.h>
+      13             : #include <mrs_lib/param_loader.h>
+      14             : 
+      15             : //}
+      16             : 
+      17             : namespace mrs_uav_controllers
+      18             : {
+      19             : 
+      20             : namespace midair_activation_controller
+      21             : {
+      22             : 
+      23             : /* class MidairActivationController //{ */
+      24             : 
+      25             : class MidairActivationController : public mrs_uav_managers::Controller {
+      26             : 
+      27             : public:
+      28             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      29             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      30             : 
+      31             :   bool activate(const ControlOutput &last_control_output);
+      32             : 
+      33             :   void deactivate(void);
+      34             : 
+      35             :   void updateInactive(const mrs_msgs::UavState &uav_state, const std::optional<mrs_msgs::TrackerCommand> &tracker_command);
+      36             : 
+      37             :   ControlOutput updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+      38             : 
+      39             :   const mrs_msgs::ControllerStatus getStatus();
+      40             : 
+      41             :   void switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      42             : 
+      43             :   void resetDisturbanceEstimators(void);
+      44             : 
+      45             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      46             : 
+      47             : private:
+      48             :   ros::NodeHandle nh_;
+      49             : 
+      50             :   bool is_initialized_ = false;
+      51             :   bool is_active_      = false;
+      52             : 
+      53             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      54             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      55             : 
+      56             :   // | ------------------------ uav state ----------------------- |
+      57             : 
+      58             :   mrs_msgs::UavState uav_state_;
+      59             :   std::mutex         mutex_uav_state_;
+      60             : 
+      61             :   // | --------------------- thrust control --------------------- |
+      62             : 
+      63             :   double _uav_mass_;
+      64             :   double uav_mass_difference_;
+      65             : 
+      66             :   double hover_throttle_;
+      67             : 
+      68             :   // | ------------------------ profiler ------------------------ |
+      69             : 
+      70             :   mrs_lib::Profiler profiler_;
+      71             :   bool              _profiler_enabled_ = false;
+      72             : 
+      73             :   // | ------------------------ routines ------------------------ |
+      74             : 
+      75             :   double getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+      76             : };
+      77             : 
+      78             : //}
+      79             : 
+      80             : // --------------------------------------------------------------
+      81             : // |                   controller's interface                   |
+      82             : // --------------------------------------------------------------
+      83             : 
+      84             : /* initialize() //{ */
+      85             : 
+      86          94 : bool MidairActivationController::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      87             :                                             std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+      88             : 
+      89          94 :   nh_ = nh;
+      90             : 
+      91          94 :   common_handlers_  = common_handlers;
+      92          94 :   private_handlers_ = private_handlers;
+      93             : 
+      94          94 :   _uav_mass_ = common_handlers->getMass();
+      95             : 
+      96          94 :   ros::Time::waitForValid();
+      97             : 
+      98             :   // | ---------- loading params using the parent's nh ---------- |
+      99             : 
+     100         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     101             : 
+     102          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     103             : 
+     104          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     105           0 :     ROS_ERROR("[MidairActivationController]: Could not load all parameters!");
+     106           0 :     return false;
+     107             :   }
+     108             : 
+     109             :   // | -------------------- loading my params ------------------- |
+     110             : 
+     111          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/midair_activation_controller.yaml");
+     112          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/midair_activation_controller.yaml");
+     113             : 
+     114          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     115           0 :     ROS_ERROR("[MidairActivationController]: Could not load all parameters!");
+     116           0 :     return false;
+     117             :   }
+     118             : 
+     119          94 :   uav_mass_difference_ = 0;
+     120             : 
+     121             :   // | ------------------------ profiler ------------------------ |
+     122             : 
+     123          94 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationController", _profiler_enabled_);
+     124             : 
+     125             :   // | ----------------------- finish init ---------------------- |
+     126             : 
+     127          94 :   ROS_INFO("[MidairActivationController]: initialized");
+     128             : 
+     129          94 :   is_initialized_ = true;
+     130             : 
+     131          94 :   return true;
+     132             : }
+     133             : 
+     134             : //}
+     135             : 
+     136             : /* activate() //{ */
+     137             : 
+     138         135 : bool MidairActivationController::activate([[maybe_unused]] const ControlOutput &last_control_output) {
+     139             : 
+     140         135 :   ROS_INFO("[MidairActivationController]: activating");
+     141             : 
+     142         135 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     143             : 
+     144         135 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     145             : 
+     146         135 :   is_active_ = true;
+     147             : 
+     148         135 :   ROS_INFO("[MidairActivationController]: activated, hover throttle %.2f", hover_throttle_);
+     149             : 
+     150         270 :   return true;
+     151             : }
+     152             : 
+     153             : //}
+     154             : 
+     155             : /* deactivate() //{ */
+     156             : 
+     157          89 : void MidairActivationController::deactivate(void) {
+     158             : 
+     159          89 :   is_active_           = false;
+     160          89 :   uav_mass_difference_ = 0;
+     161             : 
+     162          89 :   ROS_INFO("[MidairActivationController]: deactivated");
+     163          89 : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* updateInactive() //{ */
+     168             : 
+     169      136455 : void MidairActivationController::updateInactive(const mrs_msgs::UavState &                                      uav_state,
+     170             :                                                 [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     171             : 
+     172      136455 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     173      136455 : }
+     174             : 
+     175             : //}
+     176             : 
+     177             : /* //{ updateWhenAcctive() */
+     178             : 
+     179         400 : MidairActivationController::ControlOutput MidairActivationController::updateActive(const mrs_msgs::UavState &      uav_state,
+     180             :                                                                                    const mrs_msgs::TrackerCommand &tracker_command) {
+     181             : 
+     182        1200 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     183             :   mrs_lib::ScopeTimer timer =
+     184        1200 :       mrs_lib::ScopeTimer("MidairActivationController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     185             : 
+     186         400 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     187             : 
+     188         400 :   if (!is_active_) {
+     189           0 :     return Controller::ControlOutput();
+     190             :   }
+     191             : 
+     192             :   // | --------------- prepare the control output --------------- |
+     193             : 
+     194         800 :   ControlOutput control_output;
+     195             : 
+     196         400 :   control_output.diagnostics.controller = "MidairActivationController";
+     197             : 
+     198         400 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     199             : 
+     200         400 :   if (!highest_modality) {
+     201             : 
+     202           0 :     ROS_ERROR_THROTTLE(1.0, "[MidairActivationController]: output modalities are empty! This error should never appear.");
+     203             : 
+     204           0 :     return control_output;
+     205             :   }
+     206             : 
+     207         400 :   switch (highest_modality.value()) {
+     208             : 
+     209           4 :     case common::POSITION: {
+     210             : 
+     211           8 :       mrs_msgs::HwApiPositionCmd cmd;
+     212             : 
+     213           4 :       cmd.header.stamp    = ros::Time::now();
+     214           4 :       cmd.header.frame_id = uav_state.header.frame_id;
+     215             : 
+     216           4 :       cmd.position.x = uav_state.pose.position.x;
+     217           4 :       cmd.position.y = uav_state.pose.position.y;
+     218           4 :       cmd.position.z = uav_state.pose.position.z;
+     219             : 
+     220           4 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     221             : 
+     222           4 :       control_output.control_output = cmd;
+     223             : 
+     224           4 :       break;
+     225             :     }
+     226             : 
+     227           7 :     case common::VELOCITY_HDG: {
+     228             : 
+     229          14 :       mrs_msgs::HwApiVelocityHdgCmd cmd;
+     230             : 
+     231           7 :       cmd.header.stamp    = ros::Time::now();
+     232           7 :       cmd.header.frame_id = uav_state.header.frame_id;
+     233             : 
+     234           7 :       cmd.velocity.x = uav_state.velocity.linear.x;
+     235           7 :       cmd.velocity.y = uav_state.velocity.linear.y;
+     236           7 :       cmd.velocity.z = uav_state.velocity.linear.z;
+     237             : 
+     238           7 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     239             : 
+     240           7 :       control_output.control_output = cmd;
+     241             : 
+     242           7 :       break;
+     243             :     }
+     244             : 
+     245          11 :     case common::VELOCITY_HDG_RATE: {
+     246             : 
+     247          22 :       mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+     248             : 
+     249          11 :       cmd.header.stamp    = ros::Time::now();
+     250          11 :       cmd.header.frame_id = uav_state.header.frame_id;
+     251             : 
+     252          11 :       cmd.velocity.x = uav_state.velocity.linear.x;
+     253          11 :       cmd.velocity.y = uav_state.velocity.linear.y;
+     254          11 :       cmd.velocity.z = uav_state.velocity.linear.z;
+     255             : 
+     256          11 :       cmd.heading_rate = 0;
+     257             : 
+     258          11 :       control_output.control_output = cmd;
+     259             : 
+     260          11 :       break;
+     261             :     }
+     262             : 
+     263           7 :     case common::ACCELERATION_HDG: {
+     264             : 
+     265          14 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+     266             : 
+     267           7 :       cmd.header.stamp    = ros::Time::now();
+     268           7 :       cmd.header.frame_id = uav_state.header.frame_id;
+     269             : 
+     270           7 :       cmd.acceleration.x = uav_state.acceleration.linear.x;
+     271           7 :       cmd.acceleration.y = uav_state.acceleration.linear.y;
+     272           7 :       cmd.acceleration.z = uav_state.acceleration.linear.z;
+     273             : 
+     274           7 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     275             : 
+     276           7 :       control_output.control_output = cmd;
+     277             : 
+     278           7 :       break;
+     279             :     }
+     280             : 
+     281           6 :     case common::ACCELERATION_HDG_RATE: {
+     282             : 
+     283          12 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+     284             : 
+     285           6 :       cmd.header.stamp    = ros::Time::now();
+     286           6 :       cmd.header.frame_id = uav_state.header.frame_id;
+     287             : 
+     288           6 :       cmd.acceleration.x = uav_state.acceleration.linear.x;
+     289           6 :       cmd.acceleration.y = uav_state.acceleration.linear.y;
+     290           6 :       cmd.acceleration.z = uav_state.acceleration.linear.z;
+     291             : 
+     292           6 :       cmd.heading_rate = 0;
+     293             : 
+     294           6 :       control_output.control_output = cmd;
+     295             : 
+     296           6 :       break;
+     297             :     }
+     298             : 
+     299         284 :     case common::ATTITUDE: {
+     300             : 
+     301         284 :       mrs_msgs::HwApiAttitudeCmd cmd;
+     302             : 
+     303         284 :       cmd.stamp = ros::Time::now();
+     304             : 
+     305         284 :       cmd.orientation = uav_state.pose.orientation;
+     306         284 :       cmd.throttle    = hover_throttle_;
+     307             : 
+     308         284 :       control_output.control_output = cmd;
+     309             : 
+     310         284 :       break;
+     311             :     }
+     312             : 
+     313          33 :     case common::ATTITUDE_RATE: {
+     314             : 
+     315          33 :       mrs_msgs::HwApiAttitudeRateCmd cmd;
+     316             : 
+     317          33 :       cmd.stamp = ros::Time::now();
+     318             : 
+     319          33 :       cmd.body_rate.x = 0;
+     320          33 :       cmd.body_rate.y = 0;
+     321          33 :       cmd.body_rate.z = 0;
+     322             : 
+     323          33 :       cmd.throttle = hover_throttle_;
+     324             : 
+     325          33 :       control_output.control_output = cmd;
+     326             : 
+     327          33 :       break;
+     328             :     }
+     329             : 
+     330          24 :     case common::CONTROL_GROUP: {
+     331             : 
+     332          24 :       mrs_msgs::HwApiControlGroupCmd cmd;
+     333             : 
+     334          24 :       cmd.stamp = ros::Time::now();
+     335             : 
+     336          24 :       cmd.roll     = 0;
+     337          24 :       cmd.pitch    = 0;
+     338          24 :       cmd.yaw      = 0;
+     339          24 :       cmd.throttle = hover_throttle_;
+     340             : 
+     341          24 :       control_output.control_output = cmd;
+     342             : 
+     343          24 :       break;
+     344             :     }
+     345             : 
+     346          24 :     case common::ACTUATORS_CMD: {
+     347             : 
+     348          48 :       mrs_msgs::HwApiActuatorCmd cmd;
+     349             : 
+     350          24 :       cmd.stamp = ros::Time::now();
+     351             : 
+     352         120 :       for (int i = 0; i < common_handlers_->throttle_model.n_motors; i++) {
+     353          96 :         cmd.motors.push_back(hover_throttle_);
+     354             :       }
+     355             : 
+     356          24 :       control_output.control_output = cmd;
+     357             : 
+     358          24 :       break;
+     359             :     }
+     360             :   }
+     361             : 
+     362         400 :   return control_output;
+     363             : }  // namespace midair_activation_controller
+     364             : 
+     365             : //}
+     366             : 
+     367             : /* getStatus() //{ */
+     368             : 
+     369         105 : const mrs_msgs::ControllerStatus MidairActivationController::getStatus() {
+     370             : 
+     371         105 :   mrs_msgs::ControllerStatus controller_status;
+     372             : 
+     373         105 :   controller_status.active = is_active_;
+     374             : 
+     375         105 :   return controller_status;
+     376             : }
+     377             : 
+     378             : //}
+     379             : 
+     380             : /* switchOdometrySource() //{ */
+     381             : 
+     382           0 : void MidairActivationController::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     383           0 : }
+     384             : 
+     385             : //}
+     386             : 
+     387             : /* resetDisturbanceEstimators() //{ */
+     388             : 
+     389           0 : void MidairActivationController::resetDisturbanceEstimators(void) {
+     390           0 : }
+     391             : 
+     392             : //}
+     393             : 
+     394             : /* setConstraints() //{ */
+     395             : 
+     396         372 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationController::setConstraints([
+     397             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     398             : 
+     399         372 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     400             : }
+     401             : 
+     402             : //}
+     403             : 
+     404             : /* getHeadingSafely() //{ */
+     405             : 
+     406          18 : double MidairActivationController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     407             : 
+     408             :   try {
+     409          18 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     410             :   }
+     411           0 :   catch (...) {
+     412             :   }
+     413             : 
+     414             :   try {
+     415           0 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYaw();
+     416             :   }
+     417           0 :   catch (...) {
+     418             :   }
+     419             : 
+     420           0 :   if (tracker_command.use_heading) {
+     421           0 :     return tracker_command.heading;
+     422             :   }
+     423             : 
+     424           0 :   return 0;
+     425             : }
+     426             : 
+     427             : //}
+     428             : 
+     429             : }  // namespace midair_activation_controller
+     430             : 
+     431             : }  // namespace mrs_uav_controllers
+     432             : 
+     433             : #include <pluginlib/class_list_macros.h>
+     434          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_controllers::midair_activation_controller::MidairActivationController, mrs_uav_managers::Controller)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.overview.html b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.overview.html new file mode 100644 index 0000000000..ca2d6e0fdf --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.overview.html @@ -0,0 +1,129 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.png b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..49f6832acd8e49470672760900f2d06888ca2a29 GIT binary patch literal 1228 zcmV;-1T*`IP);0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vph4Hy$oLH;~?T|h?sE{F^y{nigeQwS&~b%McnB~@KD?`FrCR}1JerB z`tpc`p(qsvr?`mJYP(o^_aBZTKe^FMlq%#+_n`w!7&I8E78#6xR}J&Gq9x*3_Evvh zun+-#4j^u*0U{tcbe0W{aO#482VxHqa#70hs!`{o8eJivg0;&vg{FoIyt2JS^u7Yu1_<`hK&0f<5qD6I7WmN)ga?n_bI z7hQQSj-4jKD1hkk*v>Sa+|u7#mR}ae%ZEUEr)b}!T-?r|o{h-$QE)Rg&(n;FcK>&U zZbT)1Bi-~kYdvcm*O!E8Owl+oYI~Je)K(F7gll>VlPQ3hjf0738i48X`}jOI=c=v& zcs}AZ%Cn~!HNP zS4x-_hT_%A!1~;xGX7AQGF^>7T;=BL-AC8*Lh)`_`>5bPT!xRvwYoeS#4YgLbi+fO zay{875@xqUku0s&J(8c#rYD0VZ|AGiwOQdLg$@-Nh|@7vlgNBTOS7ERtSL&0Tp#0T z&$8&_@=p(iA6Z_5bZ|?!UvJDvrnSL@L~hOGT9ZvT-0q20JKgqNN0@R^TP3q)p{qJzPu~_kVzAwNe?obgTdz+? zqr|d=G|MVB*%t-_J~6f#3HNH4%Qy!yB-)hVZWZD#s56Rkqx14t;qB7>ESenC%X70hNB#R7%t54`RU+eO899}jWkU~WO&?XhAuq1H4}QIioLKK>RT zzjYekPM@yiNj#~T_EhYM$g3zKsWhC*+<<1{4`}^I8d=kN+6xD&hxtgAX6FEu%#-F# zV8Ga^fb2PdxYjscM1V@fv+LrD9K^I|p5l!T0tucEfls+p=UF@A1IUH`@U!_>B?K~5 zO3(XD%e~Sgc(f?}tln=go!cs8-lDlji!#SK-Eot9jccjW + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - mpc_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:72789581.2 %
Date:2024-04-25 05:42:57Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::mpc_controller::MpcController::callbackSetIntegralTerms(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_controllers::mpc_controller::MpcController::resetDisturbanceEstimators()0
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::mpc_controller::MpcController::deactivate()159
mrs_uav_controllers::mpc_controller::MpcController::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)188
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)188
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)200
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)264
mrs_uav_controllers::mpc_controller::MpcController::PIDVelocityOutput(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&, double const&)705
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)744
mrs_uav_controllers::mpc_controller::MpcController::getStatus()13311
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)14508
mrs_uav_controllers::mpc_controller::MpcController::MPC(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&)60693
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)61398
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)61662
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)114792
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)212048
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/mpc_controller.cpp.func.html b/mrs_uav_controllers/src/mpc_controller.cpp.func.html new file mode 100644 index 0000000000..90b7103310 --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - mpc_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:72789581.2 %
Date:2024-04-25 05:42:57Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::mpc_controller::MpcController::deactivate()159
mrs_uav_controllers::mpc_controller::MpcController::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)188
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)14508
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)188
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)61662
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)744
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)212048
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)61398
mrs_uav_controllers::mpc_controller::MpcController::PIDVelocityOutput(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&, double const&)705
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)114792
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)264
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
mrs_uav_controllers::mpc_controller::MpcController::callbackSetIntegralTerms(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_controllers::mpc_controller::MpcController::resetDisturbanceEstimators()0
mrs_uav_controllers::mpc_controller::MpcController::MPC(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&)60693
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)200
mrs_uav_controllers::mpc_controller::MpcController::getStatus()13311
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/mpc_controller.cpp.gcov.frameset.html b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.frameset.html new file mode 100644 index 0000000000..8a9f67a4c4 --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/src/mpc_controller.cpp.gcov.html b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.html new file mode 100644 index 0000000000..bd9a25567e --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.html @@ -0,0 +1,2217 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - mpc_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:72789581.2 %
Date:2024-04-25 05:42:57Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <common.h>
+       7             : #include <pid.hpp>
+       8             : 
+       9             : #include <mrs_uav_managers/controller.h>
+      10             : 
+      11             : #include <mpc_controller.h>
+      12             : 
+      13             : #include <dynamic_reconfigure/server.h>
+      14             : #include <mrs_uav_controllers/mpc_controllerConfig.h>
+      15             : 
+      16             : #include <std_srvs/SetBool.h>
+      17             : 
+      18             : #include <mrs_lib/profiler.h>
+      19             : #include <mrs_lib/utils.h>
+      20             : #include <mrs_lib/mutex.h>
+      21             : #include <mrs_lib/attitude_converter.h>
+      22             : #include <mrs_lib/geometry/cyclic.h>
+      23             : 
+      24             : #include <geometry_msgs/Vector3Stamped.h>
+      25             : 
+      26             : //}
+      27             : 
+      28             : #define OUTPUT_ACTUATORS 0
+      29             : #define OUTPUT_CONTROL_GROUP 1
+      30             : #define OUTPUT_ATTITUDE_RATE 2
+      31             : #define OUTPUT_ATTITUDE 3
+      32             : 
+      33             : namespace mrs_uav_controllers
+      34             : {
+      35             : 
+      36             : namespace mpc_controller
+      37             : {
+      38             : 
+      39             : /* structs //{ */
+      40             : 
+      41             : typedef struct
+      42             : {
+      43             :   double kiwxy;          // world xy integral gain
+      44             :   double kibxy;          // body xy integral gain
+      45             :   double kiwxy_lim;      // world xy integral limit
+      46             :   double kibxy_lim;      // body xy integral limit
+      47             :   double km;             // mass estimator gain
+      48             :   double km_lim;         // mass estimator limit
+      49             :   double kq_roll_pitch;  // pitch/roll attitude gain
+      50             :   double kq_yaw;         // yaw attitude gain
+      51             :   double kw_rp;          // attitude rate gain
+      52             :   double kw_y;           // attitude rate gain
+      53             : } Gains_t;
+      54             : 
+      55             : //}
+      56             : 
+      57             : /* //{ class MpcController */
+      58             : 
+      59             : class MpcController : public mrs_uav_managers::Controller {
+      60             : 
+      61             : public:
+      62             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      63             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      64             : 
+      65             :   bool activate(const ControlOutput &last_control_output);
+      66             : 
+      67             :   void deactivate(void);
+      68             : 
+      69             :   void updateInactive(const mrs_msgs::UavState &uav_state, const std::optional<mrs_msgs::TrackerCommand> &tracker_command);
+      70             : 
+      71             :   ControlOutput updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+      72             : 
+      73             :   const mrs_msgs::ControllerStatus getStatus();
+      74             : 
+      75             :   void switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      76             : 
+      77             :   void resetDisturbanceEstimators(void);
+      78             : 
+      79             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      80             : 
+      81             : private:
+      82             :   ros::NodeHandle nh_;
+      83             : 
+      84             :   bool is_initialized_ = false;
+      85             :   bool is_active_      = false;
+      86             : 
+      87             :   std::string name_;
+      88             : 
+      89             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      90             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      91             : 
+      92             :   // | ------------------------ uav state ----------------------- |
+      93             : 
+      94             :   mrs_msgs::UavState uav_state_;
+      95             :   std::mutex         mutex_uav_state_;
+      96             : 
+      97             :   // | --------------- dynamic reconfigure server --------------- |
+      98             : 
+      99             :   boost::recursive_mutex                            mutex_drs_;
+     100             :   typedef mrs_uav_controllers::mpc_controllerConfig DrsConfig_t;
+     101             :   typedef dynamic_reconfigure::Server<DrsConfig_t>  Drs_t;
+     102             :   boost::shared_ptr<Drs_t>                          drs_;
+     103             :   void                                              callbackDrs(mrs_uav_controllers::mpc_controllerConfig &config, uint32_t level);
+     104             :   DrsConfig_t                                       drs_params_;
+     105             : 
+     106             :   // | ----------------------- controllers ---------------------- |
+     107             : 
+     108             :   void positionPassthrough(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+     109             : 
+     110             :   void PIDVelocityOutput(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command, const common::CONTROL_OUTPUT &control_output,
+     111             :                          const double &dt);
+     112             : 
+     113             :   void MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command, const double &dt,
+     114             :            const common::CONTROL_OUTPUT &output_modality);
+     115             : 
+     116             :   // | ----------------------- constraints ---------------------- |
+     117             : 
+     118             :   mrs_msgs::DynamicsConstraints constraints_;
+     119             :   std::mutex                    mutex_constraints_;
+     120             : 
+     121             :   // | -------- throttle generation and mass estimation --------- |
+     122             : 
+     123             :   double _uav_mass_;
+     124             :   double uav_mass_difference_;
+     125             : 
+     126             :   Gains_t gains_;
+     127             : 
+     128             :   // | ------------------- configurable gains ------------------- |
+     129             : 
+     130             :   std::mutex mutex_gains_;       // locks the gains the are used and filtered
+     131             :   std::mutex mutex_drs_params_;  // locks the gains that came from the drs
+     132             : 
+     133             :   ros::Timer timer_gains_;
+     134             :   void       timerGains(const ros::TimerEvent &event);
+     135             : 
+     136             :   double _gain_filtering_rate_;
+     137             : 
+     138             :   // | --------------------- gain filtering --------------------- |
+     139             : 
+     140             :   double calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name, bool &updated);
+     141             : 
+     142             :   double getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
+     143             : 
+     144             :   double _gains_filter_change_rate_;
+     145             :   double _gains_filter_min_change_rate_;
+     146             : 
+     147             :   // | ----------------------- gain muting ---------------------- |
+     148             : 
+     149             :   std::atomic<bool> mute_gains_            = false;
+     150             :   std::atomic<bool> mute_gains_by_tracker_ = false;
+     151             :   double            _gain_mute_coefficient_;
+     152             : 
+     153             :   // | ------------ controller limits and saturations ----------- |
+     154             : 
+     155             :   bool   _tilt_angle_failsafe_enabled_;
+     156             :   double _tilt_angle_failsafe_;
+     157             : 
+     158             :   double _throttle_saturation_;
+     159             : 
+     160             :   // | ------------------ activation and output ----------------- |
+     161             : 
+     162             :   ControlOutput last_control_output_;
+     163             :   ControlOutput activation_control_output_;
+     164             : 
+     165             :   ros::Time         last_update_time_;
+     166             :   std::atomic<bool> first_iteration_ = true;
+     167             : 
+     168             :   // | ----------------- integral terms enabler ----------------- |
+     169             : 
+     170             :   ros::ServiceServer service_set_integral_terms_;
+     171             :   bool               callbackSetIntegralTerms(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+     172             :   bool               integral_terms_enabled_ = true;
+     173             : 
+     174             :   // | --------------------- MPC controller --------------------- |
+     175             : 
+     176             :   // number of states
+     177             :   int _n_states_;
+     178             : 
+     179             :   // time steps
+     180             :   double _dt1_;  // the first time step
+     181             :   double _dt2_;  // all the other steps
+     182             : 
+     183             :   // the last control input
+     184             :   double mpc_solver_x_u_ = 0;
+     185             :   double mpc_solver_y_u_ = 0;
+     186             :   double mpc_solver_z_u_ = 0;
+     187             : 
+     188             :   int _horizon_length_;
+     189             : 
+     190             :   // constraints
+     191             :   double _max_speed_horizontal_, _max_acceleration_horizontal_, _max_jerk_;
+     192             :   double _max_speed_vertical_, _max_acceleration_vertical_, _max_u_vertical_;
+     193             : 
+     194             :   // Q and S matrix diagonals for horizontal
+     195             :   std::vector<double> _mat_Q_, _mat_S_;
+     196             : 
+     197             :   // Q and S matrix diagonals for vertical
+     198             :   std::vector<double> _mat_Q_z_, _mat_S_z_;
+     199             : 
+     200             :   // MPC solver handlers
+     201             :   std::shared_ptr<mrs_mpc_solvers::mpc_controller::Solver> mpc_solver_x_;
+     202             :   std::shared_ptr<mrs_mpc_solvers::mpc_controller::Solver> mpc_solver_y_;
+     203             :   std::shared_ptr<mrs_mpc_solvers::mpc_controller::Solver> mpc_solver_z_;
+     204             : 
+     205             :   // MPC solver params
+     206             :   bool _mpc_solver_verbose_ = false;
+     207             :   int  _mpc_solver_max_iterations_;
+     208             : 
+     209             :   // | ------------------------ profiler ------------------------ |
+     210             : 
+     211             :   mrs_lib::Profiler profiler_;
+     212             :   bool              _profiler_enabled_ = false;
+     213             : 
+     214             :   // | ------------------------ integrals ----------------------- |
+     215             : 
+     216             :   Eigen::Vector2d Ib_b_;  // body error integral in the body frame
+     217             :   Eigen::Vector2d Iw_w_;  // world error integral in the world_frame
+     218             :   std::mutex      mutex_integrals_;
+     219             : 
+     220             :   // | ------------------------- rampup ------------------------- |
+     221             : 
+     222             :   bool   _rampup_enabled_ = false;
+     223             :   double _rampup_speed_;
+     224             : 
+     225             :   bool      rampup_active_ = false;
+     226             :   double    rampup_throttle_;
+     227             :   int       rampup_direction_;
+     228             :   double    rampup_duration_;
+     229             :   ros::Time rampup_start_time_;
+     230             :   ros::Time rampup_last_time_;
+     231             : 
+     232             :   // | ---------------------- position pid ---------------------- |
+     233             : 
+     234             :   double _pos_pid_p_;
+     235             :   double _pos_pid_i_;
+     236             :   double _pos_pid_d_;
+     237             : 
+     238             :   double _hdg_pid_p_;
+     239             :   double _hdg_pid_i_;
+     240             :   double _hdg_pid_d_;
+     241             : 
+     242             :   PIDController position_pid_x_;
+     243             :   PIDController position_pid_y_;
+     244             :   PIDController position_pid_z_;
+     245             :   PIDController position_pid_heading_;
+     246             : };
+     247             : 
+     248             : //}
+     249             : 
+     250             : // --------------------------------------------------------------
+     251             : // |                   controller's interface                   |
+     252             : // --------------------------------------------------------------
+     253             : 
+     254             : /* //{ initialize() */
+     255             : 
+     256         188 : bool MpcController::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     257             :                                std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     258             : 
+     259         188 :   nh_ = nh;
+     260             : 
+     261         188 :   common_handlers_  = common_handlers;
+     262         188 :   private_handlers_ = private_handlers;
+     263             : 
+     264         188 :   _uav_mass_ = common_handlers_->getMass();
+     265         188 :   name_      = private_handlers_->runtime_name;
+     266             : 
+     267         188 :   ros::Time::waitForValid();
+     268             : 
+     269             :   // | ---------- loading params using the parent's nh ---------- |
+     270             : 
+     271         376 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     272             : 
+     273         188 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     274             : 
+     275         188 :   if (!param_loader_parent.loadedSuccessfully()) {
+     276           0 :     ROS_ERROR("[%s]: Could not load all parameters!", name_.c_str());
+     277           0 :     return false;
+     278             :   }
+     279             : 
+     280             :   // | -------------------- loading my params ------------------- |
+     281             : 
+     282         188 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/mpc_controller.yaml");
+     283         188 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/mpc_controller.yaml");
+     284         188 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/" + private_handlers->name_space + ".yaml");
+     285         188 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/" + private_handlers->name_space + ".yaml");
+     286             : 
+     287         376 :   const std::string yaml_namespace = "mrs_uav_controllers/" + private_handlers_->name_space + "/";
+     288             : 
+     289             :   // load the dynamicall model parameters
+     290         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/number_of_states", _n_states_);
+     291         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt1", _dt1_);
+     292         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt2", _dt2_);
+     293             : 
+     294         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizon_length", _horizon_length_);
+     295             : 
+     296         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_speed", _max_speed_horizontal_);
+     297         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_acceleration", _max_acceleration_horizontal_);
+     298         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_jerk", _max_jerk_);
+     299             : 
+     300         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/Q", _mat_Q_);
+     301         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/S", _mat_S_);
+     302             : 
+     303         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_speed", _max_speed_vertical_);
+     304         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_acceleration", _max_acceleration_vertical_);
+     305         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_u", _max_u_vertical_);
+     306             : 
+     307         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/Q", _mat_Q_z_);
+     308         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/S", _mat_S_z_);
+     309             : 
+     310         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/verbose", _mpc_solver_verbose_);
+     311         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/max_iterations", _mpc_solver_max_iterations_);
+     312             : 
+     313             :   // | ------------------------- rampup ------------------------- |
+     314             : 
+     315         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/enabled", _rampup_enabled_);
+     316         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/speed", _rampup_speed_);
+     317             : 
+     318             :   // | --------------------- integral gains --------------------- |
+     319             : 
+     320         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw", gains_.kiwxy);
+     321         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kib", gains_.kibxy);
+     322             : 
+     323             :   // integrator limits
+     324         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw_lim", gains_.kiwxy_lim);
+     325         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kib_lim", gains_.kibxy_lim);
+     326             : 
+     327             :   // | ------------- height and attitude controller ------------- |
+     328             : 
+     329             :   // attitude gains
+     330         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/roll_pitch", gains_.kq_roll_pitch);
+     331         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/yaw", gains_.kq_yaw);
+     332             : 
+     333             :   // attitude rate gains
+     334         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/roll_pitch", gains_.kw_rp);
+     335         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/yaw", gains_.kw_y);
+     336             : 
+     337             :   // mass estimator
+     338         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km", gains_.km);
+     339         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km_lim", gains_.km_lim);
+     340             : 
+     341             :   // constraints
+     342         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     343         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     344             : 
+     345         188 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     346             : 
+     347         188 :   if (_tilt_angle_failsafe_enabled_ && std::abs(_tilt_angle_failsafe_) < 1e-3) {
+     348           0 :     ROS_ERROR("[%s]: constraints/tilt_angle_failsafe/enabled = 'TRUE' but the limit is too low", name_.c_str());
+     349           0 :     return false;
+     350             :   }
+     351             : 
+     352         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/throttle_saturation", _throttle_saturation_);
+     353             : 
+     354             :   // gain filtering
+     355         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     356         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     357         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/rate", _gain_filtering_rate_);
+     358         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     359             : 
+     360             :   // angular rate feed forward
+     361         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     362             : 
+     363             :   // output mode
+     364         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/preferred_output", drs_params_.preferred_output_mode);
+     365             : 
+     366             :   // | ------------------- position pid params ------------------ |
+     367             : 
+     368         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     369         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     370         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     371             : 
+     372         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     373         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     374         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     375             : 
+     376         188 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     377           0 :     ROS_ERROR("[%s]: Could not load all parameters!", this->name_.c_str());
+     378           0 :     return false;
+     379             :   }
+     380             : 
+     381             :   // | ---------------- prepare stuff from params --------------- |
+     382             : 
+     383         188 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     384         188 :         drs_params_.preferred_output_mode == OUTPUT_ATTITUDE_RATE || drs_params_.preferred_output_mode == OUTPUT_ATTITUDE)) {
+     385           0 :     ROS_ERROR("[%s]: preferred output mode has to be {0, 1, 2, 3}!", this->name_.c_str());
+     386           0 :     return false;
+     387             :   }
+     388             : 
+     389         188 :   uav_mass_difference_ = 0;
+     390         188 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     391         188 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     392             : 
+     393             :   // | ----------------- prepare the MPC solver ----------------- |
+     394             : 
+     395         188 :   mpc_solver_x_ = std::make_shared<mrs_mpc_solvers::mpc_controller::Solver>(name_, _mpc_solver_verbose_, _mpc_solver_max_iterations_, _mat_Q_, _mat_S_, _dt1_,
+     396         188 :                                                                             _dt2_, 0, 1.0);
+     397         188 :   mpc_solver_y_ = std::make_shared<mrs_mpc_solvers::mpc_controller::Solver>(name_, _mpc_solver_verbose_, _mpc_solver_max_iterations_, _mat_Q_, _mat_S_, _dt1_,
+     398         188 :                                                                             _dt2_, 0, 1.0);
+     399         188 :   mpc_solver_z_ = std::make_shared<mrs_mpc_solvers::mpc_controller::Solver>(name_, _mpc_solver_verbose_, _mpc_solver_max_iterations_, _mat_Q_z_, _mat_S_z_,
+     400         188 :                                                                             _dt1_, _dt2_, 0.5, 0.5);
+     401             : 
+     402             :   // | --------------- dynamic reconfigure server --------------- |
+     403             : 
+     404         188 :   drs_params_.kiwxy         = gains_.kiwxy;
+     405         188 :   drs_params_.kibxy         = gains_.kibxy;
+     406         188 :   drs_params_.kq_roll_pitch = gains_.kq_roll_pitch;
+     407         188 :   drs_params_.kq_yaw        = gains_.kq_yaw;
+     408         188 :   drs_params_.km            = gains_.km;
+     409         188 :   drs_params_.km_lim        = gains_.km_lim;
+     410         188 :   drs_params_.kiwxy_lim     = gains_.kiwxy_lim;
+     411         188 :   drs_params_.kibxy_lim     = gains_.kibxy_lim;
+     412             : 
+     413         188 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     414         188 :   drs_->updateConfig(drs_params_);
+     415         188 :   Drs_t::CallbackType f = boost::bind(&MpcController::callbackDrs, this, _1, _2);
+     416         188 :   drs_->setCallback(f);
+     417             : 
+     418             :   // | --------------------- service servers -------------------- |
+     419             : 
+     420         188 :   service_set_integral_terms_ = nh_.advertiseService("set_integral_terms_in", &MpcController::callbackSetIntegralTerms, this);
+     421             : 
+     422             :   // | ------------------------- timers ------------------------- |
+     423             : 
+     424         188 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &MpcController::timerGains, this, false, false);
+     425             : 
+     426             :   // | ---------------------- position pid ---------------------- |
+     427             : 
+     428         188 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     429         188 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     430         188 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     431         188 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     432             : 
+     433             :   // | ------------------------ profiler ------------------------ |
+     434             : 
+     435         188 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MpcController", _profiler_enabled_);
+     436             : 
+     437             :   // | ----------------------- finish init ---------------------- |
+     438             : 
+     439         188 :   ROS_INFO("[%s]: initialized", this->name_.c_str());
+     440             : 
+     441         188 :   is_initialized_ = true;
+     442             : 
+     443         188 :   return true;
+     444             : }
+     445             : 
+     446             : //}
+     447             : 
+     448             : /* //{ activate() */
+     449             : 
+     450         200 : bool MpcController::activate(const ControlOutput &last_control_output) {
+     451             : 
+     452         200 :   activation_control_output_ = last_control_output;
+     453             : 
+     454         200 :   double activation_mass = _uav_mass_;
+     455             : 
+     456         200 :   if (activation_control_output_.diagnostics.mass_estimator) {
+     457           7 :     uav_mass_difference_ = activation_control_output_.diagnostics.mass_difference;
+     458           7 :     activation_mass += uav_mass_difference_;
+     459           7 :     ROS_INFO("[%s]: setting mass difference from the last control output: %.2f kg", this->name_.c_str(), uav_mass_difference_);
+     460             :   }
+     461             : 
+     462         200 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     463             : 
+     464         200 :   if (activation_control_output_.diagnostics.disturbance_estimator) {
+     465           7 :     Ib_b_(0) = -activation_control_output_.diagnostics.disturbance_bx_b;
+     466           7 :     Ib_b_(1) = -activation_control_output_.diagnostics.disturbance_by_b;
+     467             : 
+     468           7 :     Iw_w_(0) = -activation_control_output_.diagnostics.disturbance_wx_w;
+     469           7 :     Iw_w_(1) = -activation_control_output_.diagnostics.disturbance_wy_w;
+     470             : 
+     471           7 :     ROS_INFO(
+     472             :         "[%s]: setting disturbances from the last control output: Ib_b_: %.2f, %.2f N, Iw_w_: "
+     473             :         "%.2f, %.2f N",
+     474             :         this->name_.c_str(), Ib_b_(0), Ib_b_(1), Iw_w_(0), Iw_w_(1));
+     475             :   }
+     476             : 
+     477             :   // did the last controller use manual throttle control?
+     478         200 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     479             : 
+     480             :   // rampup check
+     481         200 :   if (_rampup_enabled_ && throttle_last_controller) {
+     482             : 
+     483          70 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     484             : 
+     485          70 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     486             : 
+     487          70 :     if (throttle_difference > 0) {
+     488          35 :       rampup_direction_ = 1;
+     489          35 :     } else if (throttle_difference < 0) {
+     490           3 :       rampup_direction_ = -1;
+     491             :     } else {
+     492          32 :       rampup_direction_ = 0;
+     493             :     }
+     494             : 
+     495          70 :     ROS_INFO("[%s]: activating rampup with initial throttle: %.4f, target: %.4f", name_.c_str(), throttle_last_controller.value(), hover_throttle);
+     496             : 
+     497          70 :     rampup_active_     = true;
+     498          70 :     rampup_start_time_ = ros::Time::now();
+     499          70 :     rampup_last_time_  = ros::Time::now();
+     500          70 :     rampup_throttle_   = throttle_last_controller.value();
+     501             : 
+     502          70 :     rampup_duration_ = std::abs(throttle_difference) / _rampup_speed_;
+     503             :   }
+     504             : 
+     505         200 :   first_iteration_ = true;
+     506         200 :   mute_gains_      = true;
+     507             : 
+     508         200 :   timer_gains_.start();
+     509             : 
+     510         200 :   ROS_INFO("[%s]: activated", this->name_.c_str());
+     511             : 
+     512         200 :   is_active_ = true;
+     513             : 
+     514         200 :   return true;
+     515             : }
+     516             : 
+     517             : //}
+     518             : 
+     519             : /* //{ deactivate() */
+     520             : 
+     521         159 : void MpcController::deactivate(void) {
+     522             : 
+     523         159 :   is_active_           = false;
+     524         159 :   first_iteration_     = false;
+     525         159 :   uav_mass_difference_ = 0;
+     526             : 
+     527         159 :   timer_gains_.stop();
+     528             : 
+     529         159 :   ROS_INFO("[%s]: deactivated", this->name_.c_str());
+     530         159 : }
+     531             : 
+     532             : //}
+     533             : 
+     534             : /* updateInactive() //{ */
+     535             : 
+     536      212048 : void MpcController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     537             : 
+     538      212048 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     539             : 
+     540      212048 :   last_update_time_ = uav_state.header.stamp;
+     541             : 
+     542      212048 :   first_iteration_ = false;
+     543      212048 : }
+     544             : 
+     545             : //}
+     546             : 
+     547             : /* //{ updateWhenAcctive() */
+     548             : 
+     549       61662 : MpcController::ControlOutput MpcController::updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     550             : 
+     551      184986 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateActive");
+     552      184986 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcController::updateActive", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     553             : 
+     554      123324 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     555             : 
+     556       61662 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     557             : 
+     558       61662 :   last_control_output_.desired_heading_rate          = {};
+     559       61662 :   last_control_output_.desired_orientation           = {};
+     560       61662 :   last_control_output_.desired_unbiased_acceleration = {};
+     561       61662 :   last_control_output_.control_output                = {};
+     562             : 
+     563       61662 :   if (!is_active_) {
+     564           0 :     return last_control_output_;
+     565             :   }
+     566             : 
+     567             :   // | -------------------- calculate the dt -------------------- |
+     568             : 
+     569             :   double dt;
+     570             : 
+     571       61662 :   if (first_iteration_) {
+     572          69 :     dt               = 0.01;
+     573          69 :     first_iteration_ = false;
+     574             :   } else {
+     575       61593 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     576             :   }
+     577             : 
+     578       61662 :   last_update_time_ = uav_state.header.stamp;
+     579             : 
+     580       61662 :   if (std::abs(dt) < 0.001) {
+     581             : 
+     582           0 :     ROS_DEBUG("[%s]: the last odometry message came too close (%.2f s)!", name_.c_str(), dt);
+     583             : 
+     584           0 :     dt = 0.01;
+     585             :   }
+     586             : 
+     587             :   // | ----------- obtain the lowest possible modality ---------- |
+     588             : 
+     589       61662 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     590             : 
+     591       61662 :   if (!lowest_modality) {
+     592             : 
+     593           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: output modalities are empty! This error should never appear.", name_.c_str());
+     594             : 
+     595           0 :     return last_control_output_;
+     596             :   }
+     597             : 
+     598             :   // | ----- we might prefer some output mode over the other ---- |
+     599             : 
+     600       61662 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     601       56345 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing attitude rate output", name_.c_str());
+     602       56345 :     lowest_modality = common::ATTITUDE_RATE;
+     603        5317 :   } else if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE && common_handlers_->control_output_modalities.attitude) {
+     604           0 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing attitude output", name_.c_str());
+     605           0 :     lowest_modality = common::ATTITUDE;
+     606        5317 :   } else if (drs_params.preferred_output_mode == OUTPUT_CONTROL_GROUP && common_handlers_->control_output_modalities.control_group) {
+     607           0 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing control group output", name_.c_str());
+     608           0 :     lowest_modality = common::CONTROL_GROUP;
+     609        5317 :   } else if (drs_params.preferred_output_mode == OUTPUT_ACTUATORS && common_handlers_->control_output_modalities.actuators) {
+     610           0 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing actuators output", name_.c_str());
+     611           0 :     lowest_modality = common::ACTUATORS_CMD;
+     612             :   }
+     613             : 
+     614       61662 :   switch (lowest_modality.value()) {
+     615             : 
+     616         264 :     case common::POSITION: {
+     617         264 :       positionPassthrough(uav_state, tracker_command);
+     618         264 :       break;
+     619             :     }
+     620             : 
+     621         229 :     case common::VELOCITY_HDG: {
+     622         229 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     623         229 :       break;
+     624             :     }
+     625             : 
+     626         476 :     case common::VELOCITY_HDG_RATE: {
+     627         476 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     628         476 :       break;
+     629             :     }
+     630             : 
+     631         504 :     case common::ACCELERATION_HDG: {
+     632         504 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     633         504 :       break;
+     634             :     }
+     635             : 
+     636         480 :     case common::ACCELERATION_HDG_RATE: {
+     637         480 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     638         480 :       break;
+     639             :     }
+     640             : 
+     641        1058 :     case common::ATTITUDE: {
+     642        1058 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE);
+     643        1058 :       break;
+     644             :     }
+     645             : 
+     646       56345 :     case common::ATTITUDE_RATE: {
+     647       56345 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     648       56345 :       break;
+     649             :     }
+     650             : 
+     651        1158 :     case common::CONTROL_GROUP: {
+     652        1158 :       MPC(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     653        1158 :       break;
+     654             :     }
+     655             : 
+     656        1148 :     case common::ACTUATORS_CMD: {
+     657        1148 :       MPC(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     658        1148 :       break;
+     659             :     }
+     660             : 
+     661           0 :     default: {
+     662           0 :       break;
+     663             :     }
+     664             :   }
+     665             : 
+     666       61662 :   return last_control_output_;
+     667             : }
+     668             : 
+     669             : //}
+     670             : 
+     671             : /* //{ getStatus() */
+     672             : 
+     673       13311 : const mrs_msgs::ControllerStatus MpcController::getStatus() {
+     674             : 
+     675       13311 :   mrs_msgs::ControllerStatus controller_status;
+     676             : 
+     677       13311 :   controller_status.active = is_active_;
+     678             : 
+     679       13311 :   return controller_status;
+     680             : }
+     681             : 
+     682             : //}
+     683             : 
+     684             : /* switchOdometrySource() //{ */
+     685             : 
+     686           5 : void MpcController::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     687             : 
+     688           5 :   ROS_INFO("[%s]: switching the odometry source", this->name_.c_str());
+     689             : 
+     690          10 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     691             : 
+     692             :   // | ----- transform world disturabances to the new frame ----- |
+     693             : 
+     694          10 :   geometry_msgs::Vector3Stamped world_integrals;
+     695             : 
+     696           5 :   world_integrals.header.stamp    = ros::Time::now();
+     697           5 :   world_integrals.header.frame_id = uav_state.header.frame_id;
+     698             : 
+     699           5 :   world_integrals.vector.x = Iw_w_(0);
+     700           5 :   world_integrals.vector.y = Iw_w_(1);
+     701           5 :   world_integrals.vector.z = 0;
+     702             : 
+     703          10 :   auto res = common_handlers_->transformer->transformSingle(world_integrals, new_uav_state.header.frame_id);
+     704             : 
+     705           5 :   if (res) {
+     706             : 
+     707           5 :     std::scoped_lock lock(mutex_integrals_);
+     708             : 
+     709           5 :     Iw_w_(0) = res.value().vector.x;
+     710           5 :     Iw_w_(1) = res.value().vector.y;
+     711             : 
+     712             :   } else {
+     713             : 
+     714           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: could not transform world integral to the new frame", this->name_.c_str());
+     715             : 
+     716           0 :     std::scoped_lock lock(mutex_integrals_);
+     717             : 
+     718           0 :     Iw_w_(0) = 0;
+     719           0 :     Iw_w_(1) = 0;
+     720             :   }
+     721           5 : }
+     722             : 
+     723             : //}
+     724             : 
+     725             : /* resetDisturbanceEstimators() //{ */
+     726             : 
+     727           0 : void MpcController::resetDisturbanceEstimators(void) {
+     728             : 
+     729           0 :   std::scoped_lock lock(mutex_integrals_);
+     730             : 
+     731           0 :   Iw_w_ = Eigen::Vector2d::Zero(2);
+     732           0 :   Ib_b_ = Eigen::Vector2d::Zero(2);
+     733           0 : }
+     734             : 
+     735             : //}
+     736             : 
+     737             : /* setConstraints() //{ */
+     738             : 
+     739         744 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcController::setConstraints([
+     740             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     741             : 
+     742         744 :   if (!is_initialized_) {
+     743           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     744             :   }
+     745             : 
+     746         744 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     747             : 
+     748         744 :   ROS_INFO("[%s]: updating constraints", this->name_.c_str());
+     749             : 
+     750        1488 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     751         744 :   res.success = true;
+     752         744 :   res.message = "constraints updated";
+     753             : 
+     754         744 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     755             : }
+     756             : 
+     757             : //}
+     758             : 
+     759             : // --------------------------------------------------------------
+     760             : // |                         controllers                        |
+     761             : // --------------------------------------------------------------
+     762             : 
+     763             : /* Mpc() //{ */
+     764             : 
+     765       60693 : void MpcController::MPC(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command, const double &dt,
+     766             :                         const common::CONTROL_OUTPUT &output_modality) {
+     767             : 
+     768      121386 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     769       60693 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     770       60693 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     771             : 
+     772             :   // | ----------------- get the current heading ---------------- |
+     773             : 
+     774       60693 :   double uav_heading = getHeadingSafely(uav_state, tracker_command);
+     775             : 
+     776             :   // | ------------------- prepare constraints ------------------ |
+     777             : 
+     778       60693 :   double max_speed_horizontal        = _max_speed_horizontal_;
+     779       60693 :   double max_speed_vertical          = _max_speed_vertical_;
+     780       60693 :   double max_acceleration_horizontal = _max_acceleration_horizontal_;
+     781       60693 :   double max_acceleration_vertical   = _max_acceleration_vertical_;
+     782       60693 :   double max_jerk                    = _max_jerk_;
+     783       60693 :   double max_u_vertical              = _max_u_vertical_;
+     784             : 
+     785       60693 :   if (tracker_command.use_full_state_prediction) {
+     786             : 
+     787       41732 :     max_speed_horizontal = 1.5 * (constraints.horizontal_speed);
+     788       41732 :     max_speed_vertical   = 1.5 * (constraints.vertical_ascending_speed < constraints.vertical_descending_speed ? constraints.vertical_ascending_speed
+     789             :                                                                                                              : constraints.vertical_descending_speed);
+     790             : 
+     791       41732 :     max_acceleration_horizontal = 1.5 * (constraints.horizontal_acceleration);
+     792       41732 :     max_acceleration_vertical =
+     793       41732 :         1.5 * (constraints.vertical_ascending_acceleration < constraints.vertical_descending_acceleration ? constraints.vertical_ascending_acceleration
+     794             :                                                                                                           : constraints.vertical_descending_acceleration);
+     795             : 
+     796       41732 :     max_jerk       = 1.5 * constraints.horizontal_jerk;
+     797       41732 :     max_u_vertical = 1.5 * (constraints.vertical_ascending_jerk < constraints.vertical_descending_jerk ? constraints.vertical_ascending_jerk
+     798             :                                                                                                        : constraints.vertical_descending_jerk);
+     799             :   }
+     800             : 
+     801             :   // --------------------------------------------------------------
+     802             :   // |          load the control reference and estimates          |
+     803             :   // --------------------------------------------------------------
+     804             : 
+     805             :   // Rp - position reference in global frame
+     806             :   // Rv - velocity reference in global frame
+     807             :   // Ra - velocity reference in global frame
+     808             : 
+     809       60693 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     810       60693 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     811       60693 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     812             : 
+     813       60693 :   Rp << tracker_command.position.x, tracker_command.position.y, tracker_command.position.z;  // fill the desired position
+     814       60693 :   Rv << tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z;
+     815             : 
+     816             :   // | ------ store the estimated values from the uav state ----- |
+     817             : 
+     818             :   // Op - position in global frame
+     819             :   // Ov - velocity in global frame
+     820       60693 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     821       60693 :   Eigen::Vector3d Ov(uav_state.velocity.linear.x, uav_state.velocity.linear.y, uav_state.velocity.linear.z);
+     822             : 
+     823             :   // R - current uav attitude
+     824       60693 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     825             : 
+     826             :   // Ow - UAV angular rate
+     827       60693 :   Eigen::Vector3d Ow(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     828             : 
+     829             :   // --------------------------------------------------------------
+     830             :   // |                     MPC lateral control                    |
+     831             :   // --------------------------------------------------------------
+     832             : 
+     833             :   // | --------------- calculate the control erros -------------- |
+     834             : 
+     835       60693 :   Eigen::Vector3d Ep = Rp - Op;
+     836       60693 :   Eigen::Vector3d Ev = Rv - Ov;
+     837             : 
+     838             :   // | ------------------- initial conditions ------------------- |
+     839             : 
+     840      121386 :   Eigen::MatrixXd initial_x = Eigen::MatrixXd::Zero(3, 1);
+     841      121386 :   Eigen::MatrixXd initial_y = Eigen::MatrixXd::Zero(3, 1);
+     842      121386 :   Eigen::MatrixXd initial_z = Eigen::MatrixXd::Zero(3, 1);
+     843             : 
+     844             :   /* initial x //{ */
+     845             : 
+     846             :   {
+     847             :     double acceleration;
+     848             :     double velocity;
+     849       60693 :     double coef = 1.5;
+     850             : 
+     851       60693 :     if (std::abs(uav_state.acceleration.linear.x) < coef * max_acceleration_horizontal) {
+     852       60693 :       acceleration = uav_state.acceleration.linear.x;
+     853             :     } else {
+     854           0 :       acceleration = tracker_command.acceleration.x;
+     855             : 
+     856           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: odometry x acceleration exceeds constraints (%.2f > %.1f * %.2f m), using reference for initial condition", name_.c_str(),
+     857             :                          std::abs(uav_state.acceleration.linear.x), coef, max_acceleration_horizontal);
+     858             :     }
+     859             : 
+     860       60693 :     if (std::abs(uav_state.velocity.linear.x) < coef * max_speed_horizontal) {
+     861       60693 :       velocity = uav_state.velocity.linear.x;
+     862             :     } else {
+     863           0 :       velocity = tracker_command.velocity.x;
+     864             : 
+     865           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: odometry x velocity exceeds constraints (%.2f > %0.1f * %.2f m), using reference for initial condition", name_.c_str(),
+     866             :                          std::abs(uav_state.velocity.linear.x), coef, max_speed_horizontal);
+     867             :     }
+     868             : 
+     869       60693 :     initial_x << uav_state.pose.position.x, velocity, acceleration;
+     870             :   }
+     871             : 
+     872             :   //}
+     873             : 
+     874             :   /* initial y //{ */
+     875             : 
+     876             :   {
+     877             :     double acceleration;
+     878             :     double velocity;
+     879       60693 :     double coef = 1.5;
+     880             : 
+     881       60693 :     if (std::abs(uav_state.acceleration.linear.y) < coef * max_acceleration_horizontal) {
+     882       60693 :       acceleration = uav_state.acceleration.linear.y;
+     883             :     } else {
+     884           0 :       acceleration = tracker_command.acceleration.y;
+     885             : 
+     886           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: odometry y acceleration exceeds constraints (%.2f > %.1f * %.2f m), using reference for initial condition", name_.c_str(),
+     887             :                          std::abs(uav_state.acceleration.linear.y), coef, max_acceleration_horizontal);
+     888             :     }
+     889             : 
+     890       60693 :     if (std::abs(uav_state.velocity.linear.y) < coef * max_speed_horizontal) {
+     891       60693 :       velocity = uav_state.velocity.linear.y;
+     892             :     } else {
+     893           0 :       velocity = tracker_command.velocity.y;
+     894             : 
+     895           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: odometry y velocity exceeds constraints (%.2f > %0.1f * %.2f m), using reference for initial condition", name_.c_str(),
+     896             :                          std::abs(uav_state.velocity.linear.y), coef, max_speed_horizontal);
+     897             :     }
+     898             : 
+     899       60693 :     initial_y << uav_state.pose.position.y, velocity, acceleration;
+     900             :   }
+     901             : 
+     902             :   //}
+     903             : 
+     904             :   /* initial z //{ */
+     905             : 
+     906             :   {
+     907             :     double acceleration;
+     908             :     double velocity;
+     909       60693 :     double coef = 1.5;
+     910             : 
+     911       60693 :     if (std::abs(uav_state.acceleration.linear.z) < coef * max_acceleration_horizontal) {
+     912       60693 :       acceleration = uav_state.acceleration.linear.z;
+     913             :     } else {
+     914           0 :       acceleration = tracker_command.acceleration.z;
+     915             : 
+     916           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: odometry z acceleration exceeds constraints (%.2f > %.1f * %.2f m), using reference for initial condition", name_.c_str(),
+     917             :                          std::abs(uav_state.acceleration.linear.z), coef, max_acceleration_horizontal);
+     918             :     }
+     919             : 
+     920       60693 :     if (std::abs(uav_state.velocity.linear.z) < coef * max_speed_vertical) {
+     921       60693 :       velocity = uav_state.velocity.linear.z;
+     922             :     } else {
+     923           0 :       velocity = tracker_command.velocity.z;
+     924             : 
+     925           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: odometry z velocity exceeds constraints (%.2f > %0.1f * %.2f m), using reference for initial condition", name_.c_str(),
+     926             :                          std::abs(uav_state.velocity.linear.z), coef, max_speed_vertical);
+     927             :     }
+     928             : 
+     929       60693 :     initial_z << uav_state.pose.position.z, velocity, acceleration;
+     930             :   }
+     931             : 
+     932             :   //}
+     933             : 
+     934             :   // | ---------------------- set reference --------------------- |
+     935             : 
+     936      121386 :   Eigen::MatrixXd mpc_reference_x = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     937      121386 :   Eigen::MatrixXd mpc_reference_y = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     938      121386 :   Eigen::MatrixXd mpc_reference_z = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     939             : 
+     940             :   // prepare the full reference vector
+     941       60693 :   if (tracker_command.use_full_state_prediction) {
+     942             : 
+     943       41732 :     mpc_reference_x(0, 0) = tracker_command.position.x;
+     944       41732 :     mpc_reference_y(0, 0) = tracker_command.position.y;
+     945       41732 :     mpc_reference_z(0, 0) = tracker_command.position.z;
+     946             : 
+     947             :     // TODO !! this is a very crude way of sampling from the desired full-state prediction, which only works
+     948             :     // with the MpcTracker. Rework this please.
+     949             : 
+     950     1085032 :     for (int i = 1; i < _horizon_length_; i++) {
+     951     1043300 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position.at(i).x;
+     952     1043300 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position.at(i).y;
+     953     1043300 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position.at(i).z;
+     954             :     }
+     955             : 
+     956     1085032 :     for (int i = 1; i < _horizon_length_; i++) {
+     957     1043300 :       mpc_reference_x((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity.at(i).x;
+     958     1043300 :       mpc_reference_y((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity.at(i).y;
+     959     1043300 :       mpc_reference_z((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity.at(i).z;
+     960             :     }
+     961             : 
+     962     1085032 :     for (int i = 1; i < _horizon_length_; i++) {
+     963     1043300 :       mpc_reference_x((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration.at(i).x;
+     964     1043300 :       mpc_reference_y((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration.at(i).y;
+     965     1043300 :       mpc_reference_z((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration.at(i).z;
+     966             :     }
+     967             : 
+     968             :   } else {
+     969             : 
+     970      511947 :     for (int i = 0; i < _horizon_length_; i++) {
+     971      492986 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.position.x;
+     972      492986 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.position.y;
+     973      492986 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.position.z;
+     974             :     }
+     975             :   }
+     976             : 
+     977             :   // | ------------------ set the penalizations ----------------- |
+     978             : 
+     979      121386 :   std::vector<double> temp_Q_horizontal = _mat_Q_;
+     980      121386 :   std::vector<double> temp_Q_vertical   = _mat_Q_z_;
+     981             : 
+     982      121386 :   std::vector<double> temp_S_horizontal = _mat_S_;
+     983      121386 :   std::vector<double> temp_S_vertical   = _mat_S_z_;
+     984             : 
+     985       60693 :   if (!tracker_command.use_position_horizontal) {
+     986           0 :     temp_Q_horizontal.at(0) = 0;
+     987           0 :     temp_S_horizontal.at(0) = 0;
+     988             :   }
+     989             : 
+     990       60693 :   if (!tracker_command.use_velocity_horizontal) {
+     991           0 :     temp_Q_horizontal.at(1) = 0;
+     992           0 :     temp_S_horizontal.at(1) = 0;
+     993             :   }
+     994             : 
+     995       60693 :   if (!tracker_command.use_position_vertical) {
+     996           0 :     temp_Q_vertical.at(0) = 0;
+     997           0 :     temp_S_vertical.at(0) = 0;
+     998             :   }
+     999             : 
+    1000       60693 :   if (!tracker_command.use_velocity_vertical) {
+    1001           0 :     temp_Q_vertical.at(1) = 0;
+    1002           0 :     temp_S_vertical.at(1) = 0;
+    1003             :   }
+    1004             : 
+    1005             :   // | ------------------------ optimize ------------------------ |
+    1006             : 
+    1007       60693 :   mpc_solver_x_->setQ(temp_Q_horizontal);
+    1008       60693 :   mpc_solver_x_->setS(temp_S_horizontal);
+    1009       60693 :   mpc_solver_x_->setParams();
+    1010       60693 :   mpc_solver_x_->setLastInput(mpc_solver_x_u_);
+    1011       60693 :   mpc_solver_x_->loadReference(mpc_reference_x);
+    1012       60693 :   mpc_solver_x_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1013       60693 :   mpc_solver_x_->setInitialState(initial_x);
+    1014       60693 :   [[maybe_unused]] int iters_x = mpc_solver_x_->solveMPC();
+    1015       60693 :   mpc_solver_x_u_              = mpc_solver_x_->getFirstControlInput();
+    1016             : 
+    1017       60693 :   mpc_solver_y_->setQ(temp_Q_horizontal);
+    1018       60693 :   mpc_solver_y_->setS(temp_S_horizontal);
+    1019       60693 :   mpc_solver_y_->setParams();
+    1020       60693 :   mpc_solver_y_->setLastInput(mpc_solver_y_u_);
+    1021       60693 :   mpc_solver_y_->loadReference(mpc_reference_y);
+    1022       60693 :   mpc_solver_y_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1023       60693 :   mpc_solver_y_->setInitialState(initial_y);
+    1024       60693 :   [[maybe_unused]] int iters_y = mpc_solver_y_->solveMPC();
+    1025       60693 :   mpc_solver_y_u_              = mpc_solver_y_->getFirstControlInput();
+    1026             : 
+    1027       60693 :   mpc_solver_z_->setQ(temp_Q_vertical);
+    1028       60693 :   mpc_solver_z_->setS(temp_S_vertical);
+    1029       60693 :   mpc_solver_z_->setParams();
+    1030       60693 :   mpc_solver_z_->setLastInput(mpc_solver_z_u_);
+    1031       60693 :   mpc_solver_z_->loadReference(mpc_reference_z);
+    1032       60693 :   mpc_solver_z_->setLimits(max_speed_vertical, max_acceleration_vertical, max_u_vertical, 999.0, _dt1_, _dt2_);
+    1033       60693 :   mpc_solver_z_->setInitialState(initial_z);
+    1034       60693 :   [[maybe_unused]] int iters_z = mpc_solver_z_->solveMPC();
+    1035       60693 :   mpc_solver_z_u_              = mpc_solver_z_->getFirstControlInput();
+    1036             : 
+    1037             :   // | ----------- disable lateral feedback if needed ----------- |
+    1038             : 
+    1039       60693 :   if (tracker_command.disable_position_gains) {
+    1040           0 :     mpc_solver_x_u_ = 0;
+    1041           0 :     mpc_solver_y_u_ = 0;
+    1042             :   }
+    1043             : 
+    1044             :   // | --------------------- load the gains --------------------- |
+    1045             : 
+    1046       60693 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+    1047             : 
+    1048       60693 :   Eigen::Array3d Kw(0, 0, 0);
+    1049       60693 :   Eigen::Array3d Kq;
+    1050             : 
+    1051             :   {
+    1052       60693 :     std::scoped_lock lock(mutex_gains_);
+    1053             : 
+    1054       60693 :     Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+    1055             : 
+    1056       60693 :     Kw(0) = gains.kw_rp;
+    1057       60693 :     Kw(1) = gains.kw_rp;
+    1058       60693 :     Kw(2) = gains.kw_y;
+    1059             :   }
+    1060             : 
+    1061             :   // | ---------- desired orientation matrix and force ---------- |
+    1062             : 
+    1063             :   // get body integral in the world frame
+    1064             : 
+    1065       60693 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+    1066             : 
+    1067             :   {
+    1068             : 
+    1069      121386 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+    1070             : 
+    1071       60693 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+    1072       60693 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+    1073       60693 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+    1074       60693 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+    1075       60693 :     Ib_b_stamped.vector.z        = 0;
+    1076             : 
+    1077      121386 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+    1078             : 
+    1079       60693 :     if (res) {
+    1080       60693 :       Ib_w(0) = res.value().vector.x;
+    1081       60693 :       Ib_w(1) = res.value().vector.y;
+    1082             :     } else {
+    1083           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not transform the Ib_b_ to the world frame", name_.c_str());
+    1084             :     }
+    1085             :   }
+    1086             : 
+    1087             :   // construct the desired force vector
+    1088             : 
+    1089       60693 :   if (tracker_command.use_acceleration && !tracker_command.use_full_state_prediction) {
+    1090        1732 :     Ra << tracker_command.acceleration.x + mpc_solver_x_u_, tracker_command.acceleration.y + mpc_solver_y_u_, tracker_command.acceleration.z + mpc_solver_z_u_;
+    1091             :   } else {
+    1092       58961 :     Ra << mpc_solver_x_u_, mpc_solver_y_u_, mpc_solver_z_u_;
+    1093             :   }
+    1094             : 
+    1095       60693 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+    1096             : 
+    1097       60693 :   Eigen::Vector3d feed_forward = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+    1098             : 
+    1099       60693 :   Eigen::Vector3d integral_feedback;
+    1100             :   {
+    1101       60693 :     std::scoped_lock lock(mutex_integrals_);
+    1102             : 
+    1103       60693 :     integral_feedback << Ib_w(0) + Iw_w_(0), Ib_w(1) + Iw_w_(1), 0;
+    1104             :   }
+    1105             : 
+    1106             :   // --------------------------------------------------------------
+    1107             :   // |                 integrators and estimators                 |
+    1108             :   // --------------------------------------------------------------
+    1109             : 
+    1110             :   /* world error integrator //{ */
+    1111             : 
+    1112             :   // --------------------------------------------------------------
+    1113             :   // |                  integrate the world error                 |
+    1114             :   // --------------------------------------------------------------
+    1115             : 
+    1116             :   {
+    1117      121386 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+    1118             : 
+    1119       60693 :     Eigen::Vector3d integration_switch(1, 1, 0);
+    1120             : 
+    1121             :     // integrate the world error
+    1122             : 
+    1123             :     // antiwindup
+    1124       60693 :     double temp_gain = gains.kiwxy;
+    1125       60693 :     if (!tracker_command.disable_antiwindups) {
+    1126       51986 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1127       23050 :         temp_gain = 0;
+    1128       23050 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for world integral kicks in", this->name_.c_str());
+    1129             :       }
+    1130             :     }
+    1131             : 
+    1132       60693 :     if (integral_terms_enabled_) {
+    1133       60693 :       if (tracker_command.use_position_horizontal) {
+    1134       60693 :         Iw_w_ += temp_gain * Ep.head(2) * dt;
+    1135           0 :       } else if (tracker_command.use_velocity_horizontal) {
+    1136           0 :         Iw_w_ += temp_gain * Ev.head(2) * dt;
+    1137             :       }
+    1138             :     }
+    1139             : 
+    1140             :     // saturate the world X
+    1141       60693 :     bool world_integral_saturated = false;
+    1142       60693 :     if (!std::isfinite(Iw_w_(0))) {
+    1143           0 :       Iw_w_(0) = 0;
+    1144           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable 'Iw_w_(0)', setting it to 0!!!", this->name_.c_str());
+    1145       60693 :     } else if (Iw_w_(0) > gains.kiwxy_lim) {
+    1146           0 :       Iw_w_(0)                 = gains.kiwxy_lim;
+    1147           0 :       world_integral_saturated = true;
+    1148       60693 :     } else if (Iw_w_(0) < -gains.kiwxy_lim) {
+    1149           0 :       Iw_w_(0)                 = -gains.kiwxy_lim;
+    1150           0 :       world_integral_saturated = true;
+    1151             :     }
+    1152             : 
+    1153       60693 :     if (gains.kiwxy_lim >= 0 && world_integral_saturated) {
+    1154           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: MPC's world X integral is being saturated!", this->name_.c_str());
+    1155             :     }
+    1156             : 
+    1157             :     // saturate the world Y
+    1158       60693 :     world_integral_saturated = false;
+    1159       60693 :     if (!std::isfinite(Iw_w_(1))) {
+    1160           0 :       Iw_w_(1) = 0;
+    1161           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable 'Iw_w_(1)', setting it to 0!!!", this->name_.c_str());
+    1162       60693 :     } else if (Iw_w_(1) > gains.kiwxy_lim) {
+    1163           0 :       Iw_w_(1)                 = gains.kiwxy_lim;
+    1164           0 :       world_integral_saturated = true;
+    1165       60693 :     } else if (Iw_w_(1) < -gains.kiwxy_lim) {
+    1166           0 :       Iw_w_(1)                 = -gains.kiwxy_lim;
+    1167           0 :       world_integral_saturated = true;
+    1168             :     }
+    1169             : 
+    1170       60693 :     if (gains.kiwxy_lim >= 0 && world_integral_saturated) {
+    1171           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: MPC's world Y integral is being saturated!", this->name_.c_str());
+    1172             :     }
+    1173             :   }
+    1174             : 
+    1175             :   //}
+    1176             : 
+    1177             :   /* body error integrator //{ */
+    1178             : 
+    1179             :   {
+    1180      121386 :     std::scoped_lock lock(mutex_gains_);
+    1181             : 
+    1182       60693 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+    1183       60693 :     Eigen::Vector2d Ev_fcu_untilted = Eigen::Vector2d(0, 0);  // velocity error in the untilted frame of the UAV
+    1184             : 
+    1185             :     // get the position control error in the fcu_untilted frame
+    1186             :     {
+    1187             : 
+    1188      121386 :       geometry_msgs::Vector3Stamped Ep_stamped;
+    1189             : 
+    1190       60693 :       Ep_stamped.header.stamp    = ros::Time::now();
+    1191       60693 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+    1192       60693 :       Ep_stamped.vector.x        = Ep(0);
+    1193       60693 :       Ep_stamped.vector.y        = Ep(1);
+    1194       60693 :       Ep_stamped.vector.z        = Ep(2);
+    1195             : 
+    1196      182079 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+    1197             : 
+    1198       60693 :       if (res) {
+    1199       60693 :         Ep_fcu_untilted(0) = res.value().vector.x;
+    1200       60693 :         Ep_fcu_untilted(1) = res.value().vector.y;
+    1201             :       } else {
+    1202           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not transform the position error to fcu_untilted", name_.c_str());
+    1203             :       }
+    1204             :     }
+    1205             : 
+    1206             :     // get the velocity control error in the fcu_untilted frame
+    1207             :     {
+    1208      121386 :       geometry_msgs::Vector3Stamped Ev_stamped;
+    1209             : 
+    1210       60693 :       Ev_stamped.header.stamp    = ros::Time::now();
+    1211       60693 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+    1212       60693 :       Ev_stamped.vector.x        = Ev(0);
+    1213       60693 :       Ev_stamped.vector.y        = Ev(1);
+    1214       60693 :       Ev_stamped.vector.z        = Ev(2);
+    1215             : 
+    1216      182079 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+    1217             : 
+    1218       60693 :       if (res) {
+    1219       60693 :         Ev_fcu_untilted(0) = res.value().vector.x;
+    1220       60693 :         Ev_fcu_untilted(1) = res.value().vector.x;
+    1221             :       } else {
+    1222           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not transform the velocity error to fcu_untilted", name_.c_str());
+    1223             :       }
+    1224             :     }
+    1225             : 
+    1226             :     // integrate the body error
+    1227             : 
+    1228             :     // antiwindup
+    1229       60693 :     double temp_gain = gains.kibxy;
+    1230       60693 :     if (!tracker_command.disable_antiwindups) {
+    1231       51986 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1232       23050 :         temp_gain = 0;
+    1233       23050 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for body integral kicks in", this->name_.c_str());
+    1234             :       }
+    1235             :     }
+    1236             : 
+    1237       60693 :     if (integral_terms_enabled_) {
+    1238       60693 :       if (tracker_command.use_position_horizontal) {
+    1239       60693 :         Ib_b_ += temp_gain * Ep_fcu_untilted * dt;
+    1240           0 :       } else if (tracker_command.use_velocity_horizontal) {
+    1241           0 :         Ib_b_ += temp_gain * Ev_fcu_untilted * dt;
+    1242             :       }
+    1243             :     }
+    1244             : 
+    1245             :     // saturate the body X
+    1246       60693 :     bool body_integral_saturated = false;
+    1247       60693 :     if (!std::isfinite(Ib_b_(0))) {
+    1248           0 :       Ib_b_(0) = 0;
+    1249           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable 'Ib_b_(0)', setting it to 0!!!", this->name_.c_str());
+    1250       60693 :     } else if (Ib_b_(0) > gains.kibxy_lim) {
+    1251           0 :       Ib_b_(0)                = gains.kibxy_lim;
+    1252           0 :       body_integral_saturated = true;
+    1253       60693 :     } else if (Ib_b_(0) < -gains.kibxy_lim) {
+    1254           0 :       Ib_b_(0)                = -gains.kibxy_lim;
+    1255           0 :       body_integral_saturated = true;
+    1256             :     }
+    1257             : 
+    1258       60693 :     if (gains.kibxy_lim > 0 && body_integral_saturated) {
+    1259           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: MPC's body pitch integral is being saturated!", this->name_.c_str());
+    1260             :     }
+    1261             : 
+    1262             :     // saturate the body
+    1263       60693 :     body_integral_saturated = false;
+    1264       60693 :     if (!std::isfinite(Ib_b_(1))) {
+    1265           0 :       Ib_b_(1) = 0;
+    1266           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable 'Ib_b_(1)', setting it to 0!!!", this->name_.c_str());
+    1267       60693 :     } else if (Ib_b_(1) > gains.kibxy_lim) {
+    1268           0 :       Ib_b_(1)                = gains.kibxy_lim;
+    1269           0 :       body_integral_saturated = true;
+    1270       60693 :     } else if (Ib_b_(1) < -gains.kibxy_lim) {
+    1271           0 :       Ib_b_(1)                = -gains.kibxy_lim;
+    1272           0 :       body_integral_saturated = true;
+    1273             :     }
+    1274             : 
+    1275       60693 :     if (gains.kibxy_lim > 0 && body_integral_saturated) {
+    1276           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: MPC's body roll integral is being saturated!", this->name_.c_str());
+    1277             :     }
+    1278             :   }
+    1279             : 
+    1280             :   //}
+    1281             : 
+    1282       60693 :   Eigen::Vector3d des_acc = integral_feedback / total_mass + Ra;
+    1283             : 
+    1284       60693 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1285             : 
+    1286         984 :     if (output_modality == common::ACCELERATION_HDG) {
+    1287             : 
+    1288        1008 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1289             : 
+    1290         504 :       cmd.acceleration.x = des_acc(0);
+    1291         504 :       cmd.acceleration.y = des_acc(1);
+    1292         504 :       cmd.acceleration.z = des_acc(2);
+    1293             : 
+    1294         504 :       cmd.heading = tracker_command.heading;
+    1295             : 
+    1296         504 :       last_control_output_.control_output = cmd;
+    1297             : 
+    1298             :     } else {
+    1299             : 
+    1300         480 :       double des_hdg_ff = 0;
+    1301             : 
+    1302         480 :       if (tracker_command.use_heading_rate) {
+    1303         480 :         des_hdg_ff = tracker_command.heading_rate;
+    1304             :       }
+    1305             : 
+    1306         960 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1307             : 
+    1308         480 :       cmd.acceleration.x = des_acc(0);
+    1309         480 :       cmd.acceleration.y = des_acc(1);
+    1310         480 :       cmd.acceleration.z = des_acc(2);
+    1311             : 
+    1312         480 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1313             : 
+    1314         480 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1315             : 
+    1316         480 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1317             : 
+    1318         480 :       cmd.heading_rate = des_hdg_rate;
+    1319             : 
+    1320         480 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1321             : 
+    1322         480 :       last_control_output_.control_output = cmd;
+    1323             :     }
+    1324             : 
+    1325             :     // | -------------- unbiased desired acceleration ------------- |
+    1326             : 
+    1327         984 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1328             : 
+    1329             :     {
+    1330             : 
+    1331        1968 :       geometry_msgs::Vector3Stamped world_accel;
+    1332             : 
+    1333         984 :       world_accel.header.stamp    = ros::Time::now();
+    1334         984 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1335         984 :       world_accel.vector.x        = Ra(0);
+    1336         984 :       world_accel.vector.y        = Ra(1);
+    1337         984 :       world_accel.vector.z        = Ra(2);
+    1338             : 
+    1339        2952 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1340             : 
+    1341         984 :       if (res) {
+    1342         984 :         unbiased_des_acc << res.value().vector.x, res.value().vector.y, res.value().vector.z;
+    1343             :       }
+    1344             :     }
+    1345             : 
+    1346             :     // fill the unbiased desired accelerations
+    1347         984 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1348             : 
+    1349             :     // | ----------------- fill in the diagnostics ---------------- |
+    1350             : 
+    1351         984 :     last_control_output_.diagnostics.ramping_up = false;
+    1352             : 
+    1353         984 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1354         984 :     last_control_output_.diagnostics.mass_difference = 0;
+    1355         984 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1356             : 
+    1357         984 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1358             : 
+    1359         984 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1360         984 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1361             : 
+    1362         984 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1363         984 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1364             : 
+    1365         984 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1366         984 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1367             : 
+    1368         984 :     last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1369             : 
+    1370         984 :     last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * max_speed_horizontal;
+    1371         984 :     last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * max_acceleration_horizontal;
+    1372             : 
+    1373         984 :     last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * max_speed_vertical;
+    1374         984 :     last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1375             : 
+    1376         984 :     last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * max_speed_vertical;
+    1377         984 :     last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1378             : 
+    1379         984 :     last_control_output_.diagnostics.controller = name_;
+    1380             : 
+    1381         984 :     return;
+    1382             :   }
+    1383             : 
+    1384             :   /* mass estimatior //{ */
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                integrate the mass difference               |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390             :   {
+    1391      119418 :     std::scoped_lock lock(mutex_gains_);
+    1392             : 
+    1393             :     // antiwindup
+    1394       59709 :     double temp_gain = gains.km;
+    1395      133647 :     if (rampup_active_ ||
+    1396       73938 :         (std::abs(uav_state.velocity.linear.z) > 0.3 && ((Ep(2) > 0 && uav_state.velocity.linear.z > 0) || (Ep(2) < 0 && uav_state.velocity.linear.z < 0)))) {
+    1397       14683 :       temp_gain = 0;
+    1398       14683 :       ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for the mass kicks in", this->name_.c_str());
+    1399             :     }
+    1400             : 
+    1401       59709 :     if (tracker_command.use_position_vertical) {
+    1402       59709 :       uav_mass_difference_ += temp_gain * Ep(2) * dt;
+    1403             :     }
+    1404             : 
+    1405             :     // saturate the mass estimator
+    1406       59709 :     bool uav_mass_saturated = false;
+    1407       59709 :     if (!std::isfinite(uav_mass_difference_)) {
+    1408           0 :       uav_mass_difference_ = 0;
+    1409           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in variable 'uav_mass_difference_', setting it to 0 and returning!!!", this->name_.c_str());
+    1410       59709 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1411           0 :       uav_mass_difference_ = gains.km_lim;
+    1412           0 :       uav_mass_saturated   = true;
+    1413       59709 :     } else if (uav_mass_difference_ < -gains.km_lim) {
+    1414           0 :       uav_mass_difference_ = -gains.km_lim;
+    1415           0 :       uav_mass_saturated   = true;
+    1416             :     }
+    1417             : 
+    1418       59709 :     if (uav_mass_saturated) {
+    1419           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: The UAV mass difference is being saturated to %.2f!", this->name_.c_str(), uav_mass_difference_);
+    1420             :     }
+    1421             :   }
+    1422             : 
+    1423             :   //}
+    1424             : 
+    1425       59709 :   Eigen::Vector3d f = integral_feedback + feed_forward;
+    1426             : 
+    1427             :   // | ----------- limiting the downwards acceleration ---------- |
+    1428             :   // the downwards force produced by the position and the acceleration feedback should not be larger than the gravity
+    1429             : 
+    1430             :   // if the downwards part of the force is close to counter-act the gravity acceleration
+    1431       59709 :   if (f(2) < 0) {
+    1432             : 
+    1433           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: the calculated downwards desired force is negative (%.2f) -> mitigating flip", this->name_.c_str(), f(2));
+    1434             : 
+    1435           0 :     f << 0, 0, 1;
+    1436             :   }
+    1437             : 
+    1438             :   // | ------------------- sanitize tilt angle ------------------ |
+    1439             : 
+    1440       59709 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1441             : 
+    1442       59709 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "MpcController");
+    1443             : 
+    1444       59709 :   if (!f_normed_sanitized) {
+    1445             : 
+    1446           0 :     ROS_INFO("[%s]: f = [%.2f, %.2f, %.2f]", this->name_.c_str(), f(0), f(1), f(2));
+    1447           0 :     ROS_INFO("[%s]: integral feedback: [%.2f, %.2f, %.2f]", this->name_.c_str(), integral_feedback(0), integral_feedback(1), integral_feedback(2));
+    1448           0 :     ROS_INFO("[%s]: feed forward: [%.2f, %.2f, %.2f]", this->name_.c_str(), feed_forward(0), feed_forward(1), feed_forward(2));
+    1449           0 :     ROS_INFO("[%s]: tracker_cmd: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", this->name_.c_str(), tracker_command.position.x, tracker_command.position.y,
+    1450             :              tracker_command.position.z, tracker_command.heading);
+    1451           0 :     ROS_INFO("[%s]: odometry: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", this->name_.c_str(), uav_state.pose.position.x, uav_state.pose.position.y,
+    1452             :              uav_state.pose.position.z, uav_heading);
+    1453             : 
+    1454           0 :     return;
+    1455             :   }
+    1456             : 
+    1457       59709 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1458             : 
+    1459             :   // --------------------------------------------------------------
+    1460             :   // |               desired orientation + throttle               |
+    1461             :   // --------------------------------------------------------------
+    1462             : 
+    1463             :   // | ------------------- desired orientation ------------------ |
+    1464             : 
+    1465       59709 :   Eigen::Matrix3d Rd;
+    1466             : 
+    1467       59709 :   if (tracker_command.use_orientation) {
+    1468             : 
+    1469             :     // fill in the desired orientation based on the desired orientation from the control command
+    1470           0 :     Rd = mrs_lib::AttitudeConverter(tracker_command.orientation);
+    1471             : 
+    1472           0 :     if (tracker_command.use_heading) {
+    1473             :       try {
+    1474           0 :         Rd = mrs_lib::AttitudeConverter(Rd).setHeading(tracker_command.heading);
+    1475             :       }
+    1476           0 :       catch (...) {
+    1477           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: failed to add heading to the desired orientation matrix", this->name_.c_str());
+    1478             :       }
+    1479             :     }
+    1480             : 
+    1481             :   } else {
+    1482             : 
+    1483       59709 :     Eigen::Vector3d bxd;  // desired heading vector
+    1484             : 
+    1485       59709 :     if (tracker_command.use_heading) {
+    1486       59709 :       bxd << cos(tracker_command.heading), sin(tracker_command.heading), 0;
+    1487             :     } else {
+    1488           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: desired heading was not specified, using current heading instead!", this->name_.c_str());
+    1489           0 :       bxd << cos(uav_heading), sin(uav_heading), 0;
+    1490             :     }
+    1491             : 
+    1492       59709 :     Rd = common::so3transform(f_normed, bxd, false);
+    1493             :   }
+    1494             : 
+    1495             :   // | -------------------- desired throttle -------------------- |
+    1496             : 
+    1497       59709 :   double desired_thrust_force = f.dot(R.col(2));
+    1498       59709 :   double throttle             = 0;
+    1499             : 
+    1500       59709 :   if (rampup_active_) {
+    1501             : 
+    1502             :     // deactivate the rampup when the times up
+    1503        1022 :     if (std::abs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1504             : 
+    1505          62 :       rampup_active_ = false;
+    1506             : 
+    1507          62 :       ROS_INFO("[%s]: rampup finished", name_.c_str());
+    1508             : 
+    1509             :     } else {
+    1510             : 
+    1511         960 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1512             : 
+    1513         960 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1514             : 
+    1515         960 :       rampup_last_time_ = ros::Time::now();
+    1516             : 
+    1517         960 :       throttle = rampup_throttle_;
+    1518             : 
+    1519         960 :       ROS_INFO_THROTTLE(0.1, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
+    1520             :     }
+    1521             : 
+    1522             :   } else {
+    1523             : 
+    1524       58687 :     if (desired_thrust_force >= 0) {
+    1525       58687 :       throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, desired_thrust_force);
+    1526             :     } else {
+    1527           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: just so you know, the desired throttle force is negative (%.2f)", name_.c_str(), desired_thrust_force);
+    1528             :     }
+    1529             :   }
+    1530             : 
+    1531             :   // | ------------------- throttle saturation ------------------ |
+    1532             : 
+    1533       59709 :   bool throttle_saturated = false;
+    1534             : 
+    1535       59709 :   if (!std::isfinite(throttle)) {
+    1536             : 
+    1537           0 :     ROS_ERROR("[%s]: NaN detected in variable 'throttle'!!!", name_.c_str());
+    1538           0 :     return;
+    1539             : 
+    1540       59709 :   } else if (throttle > _throttle_saturation_) {
+    1541          27 :     throttle = _throttle_saturation_;
+    1542          27 :     ROS_WARN_THROTTLE(0.1, "[%s]: saturating throttle to %.2f", name_.c_str(), _throttle_saturation_);
+    1543       59682 :   } else if (throttle < 0.0) {
+    1544           0 :     throttle = 0.0;
+    1545           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: saturating throttle to 0.0", name_.c_str());
+    1546             :   }
+    1547             : 
+    1548       59709 :   if (throttle_saturated) {
+    1549           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: ---------------------------", name_.c_str());
+    1550           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: desired state: pos [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", name_.c_str(), tracker_command.position.x,
+    1551             :                       tracker_command.position.y, tracker_command.position.z, tracker_command.heading);
+    1552           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: desired state: vel [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", name_.c_str(), tracker_command.velocity.x,
+    1553             :                       tracker_command.velocity.y, tracker_command.velocity.z, tracker_command.heading_rate);
+    1554           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: desired state: acc [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", name_.c_str(), tracker_command.acceleration.x,
+    1555             :                       tracker_command.acceleration.y, tracker_command.acceleration.z, tracker_command.heading_acceleration);
+    1556           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: desired state: jerk [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", name_.c_str(), tracker_command.jerk.x, tracker_command.jerk.y,
+    1557             :                       tracker_command.jerk.z, tracker_command.heading_jerk);
+    1558           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: ---------------------------", name_.c_str());
+    1559           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: current state: pos [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", name_.c_str(), uav_state.pose.position.x,
+    1560             :                       uav_state.pose.position.y, uav_state.pose.position.z, uav_heading);
+    1561           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: current state: vel [x: %.2f, y: %.2f, z: %.2f, yaw rate: %.2f]", name_.c_str(), uav_state.velocity.linear.x,
+    1562             :                       uav_state.velocity.linear.y, uav_state.velocity.linear.z, uav_state.velocity.angular.z);
+    1563           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: ---------------------------", name_.c_str());
+    1564             :   }
+    1565             : 
+    1566             :   // | -------------- unbiased desired acceleration ------------- |
+    1567             : 
+    1568       59709 :   double desired_x_accel = 0;
+    1569       59709 :   double desired_y_accel = 0;
+    1570       59709 :   double desired_z_accel = 0;
+    1571             : 
+    1572             :   {
+    1573       59709 :     Eigen::Vector3d thrust_vector = desired_thrust_force * Rd.col(2);
+    1574             : 
+    1575       59709 :     double world_accel_x = (thrust_vector(0) / total_mass) - (Iw_w_(0) / total_mass) - (Ib_w(0) / total_mass);
+    1576       59709 :     double world_accel_y = (thrust_vector(1) / total_mass) - (Iw_w_(1) / total_mass) - (Ib_w(1) / total_mass);
+    1577             : 
+    1578             :     // TODO change to z from IMU?
+    1579       59709 :     double world_accel_z = tracker_command.acceleration.z;
+    1580             : 
+    1581      119418 :     geometry_msgs::Vector3Stamped world_accel;
+    1582             : 
+    1583       59709 :     world_accel.header.stamp    = ros::Time::now();
+    1584       59709 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1585       59709 :     world_accel.vector.x        = world_accel_x;
+    1586       59709 :     world_accel.vector.y        = world_accel_y;
+    1587       59709 :     world_accel.vector.z        = world_accel_z;
+    1588             : 
+    1589      179127 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1590             : 
+    1591       59709 :     if (res) {
+    1592             : 
+    1593       59709 :       desired_x_accel = res.value().vector.x;
+    1594       59709 :       desired_y_accel = res.value().vector.y;
+    1595       59709 :       desired_z_accel = res.value().vector.z;
+    1596             :     }
+    1597             :   }
+    1598             : 
+    1599             :   // | --------------- fill the resulting command --------------- |
+    1600             : 
+    1601             :   // fill the desired orientation for the tilt error check
+    1602       59709 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1603             : 
+    1604             :   // fill the unbiased desired accelerations
+    1605       59709 :   last_control_output_.desired_unbiased_acceleration = Eigen::Vector3d(desired_x_accel, desired_y_accel, desired_z_accel);
+    1606             : 
+    1607             :   // | ----------------- fill in the diagnostics ---------------- |
+    1608             : 
+    1609       59709 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1610             : 
+    1611       59709 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1612       59709 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1613       59709 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1614             : 
+    1615       59709 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1616             : 
+    1617       59709 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1618       59709 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1619             : 
+    1620       59709 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1621       59709 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1622             : 
+    1623       59709 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1624       59709 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1625             : 
+    1626       59709 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1627             : 
+    1628       59709 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1629       59709 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1630             : 
+    1631       59709 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1632       59709 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1633             : 
+    1634       59709 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1635       59709 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1636             : 
+    1637       59709 :   last_control_output_.diagnostics.controller = name_;
+    1638             : 
+    1639             :   // | ------------ construct the attitude reference ------------ |
+    1640             : 
+    1641       59709 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1642             : 
+    1643       59709 :   attitude_cmd.stamp       = ros::Time::now();
+    1644       59709 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1645       59709 :   attitude_cmd.throttle    = throttle;
+    1646             : 
+    1647       59709 :   if (output_modality == common::ATTITUDE) {
+    1648             : 
+    1649        1058 :     last_control_output_.control_output = attitude_cmd;
+    1650             : 
+    1651        1058 :     return;
+    1652             :   }
+    1653             : 
+    1654             :   // --------------------------------------------------------------
+    1655             :   // |                      attitude control                      |
+    1656             :   // --------------------------------------------------------------
+    1657             : 
+    1658       58651 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1659             : 
+    1660       58651 :   if (tracker_command.use_attitude_rate) {
+    1661             : 
+    1662           0 :     rate_feedforward << tracker_command.attitude_rate.x, tracker_command.attitude_rate.y, tracker_command.attitude_rate.z;
+    1663             : 
+    1664       58651 :   } else if (tracker_command.use_heading_rate) {
+    1665             : 
+    1666             :     // to fill in the feed forward yaw rate
+    1667       58650 :     double desired_yaw_rate = 0;
+    1668             : 
+    1669             :     try {
+    1670       58650 :       desired_yaw_rate = mrs_lib::AttitudeConverter(Rd).getYawRateIntrinsic(tracker_command.heading_rate);
+    1671             :     }
+    1672           0 :     catch (...) {
+    1673           0 :       ROS_ERROR("[%s]: exception caught while calculating the desired_yaw_rate feedforward", name_.c_str());
+    1674             :     }
+    1675             : 
+    1676       58650 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1677             :   }
+    1678             : 
+    1679             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1680             : 
+    1681       58651 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1682             : 
+    1683       58651 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1684             : 
+    1685       39694 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: using jerk feedforward", name_.c_str());
+    1686             : 
+    1687       39694 :     Eigen::Matrix3d I;
+    1688       39694 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1689       39694 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1690       39694 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1691             :   }
+    1692             : 
+    1693             :   // | --------------- run the attitude controller -------------- |
+    1694             : 
+    1695       58651 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1696             : 
+    1697       58651 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq, false);
+    1698             : 
+    1699       58651 :   if (!attitude_rate_command) {
+    1700           0 :     return;
+    1701             :   }
+    1702             : 
+    1703             :   // | --------- fill in the already known attitude rate -------- |
+    1704             : 
+    1705             :   {
+    1706             :     try {
+    1707       58651 :       last_control_output_.desired_heading_rate = mrs_lib::AttitudeConverter(R).getHeadingRate(attitude_rate_command->body_rate);
+    1708             :     }
+    1709           0 :     catch (...) {
+    1710             :     }
+    1711             :   }
+    1712             : 
+    1713             :   // | ---------- construct the attitude rate reference --------- |
+    1714             : 
+    1715       58651 :   if (output_modality == common::ATTITUDE_RATE) {
+    1716             : 
+    1717       56345 :     last_control_output_.control_output = attitude_rate_command;
+    1718             : 
+    1719       56345 :     return;
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                    Attitude rate control                   |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726        2306 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1727             : 
+    1728        2306 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1729             : 
+    1730        2306 :   if (!control_group_command) {
+    1731           0 :     return;
+    1732             :   }
+    1733             : 
+    1734        2306 :   if (output_modality == common::CONTROL_GROUP) {
+    1735             : 
+    1736        1158 :     last_control_output_.control_output = control_group_command;
+    1737             : 
+    1738        1158 :     return;
+    1739             :   }
+    1740             : 
+    1741             :   // --------------------------------------------------------------
+    1742             :   // |                        output mixer                        |
+    1743             :   // --------------------------------------------------------------
+    1744             : 
+    1745        2296 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1746             : 
+    1747        1148 :   last_control_output_.control_output = actuator_cmd;
+    1748             : 
+    1749        1148 :   return;
+    1750             : }
+    1751             : 
+    1752             : //}
+    1753             : 
+    1754             : /* positionPassthrough() //{ */
+    1755             : 
+    1756         264 : void MpcController::positionPassthrough(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    1757             : 
+    1758         264 :   if (!tracker_command.use_position_vertical || !tracker_command.use_position_horizontal || !tracker_command.use_heading) {
+    1759           0 :     ROS_ERROR("[%s]: the tracker did not provide position+hdg reference", name_.c_str());
+    1760           0 :     return;
+    1761             :   }
+    1762             : 
+    1763         528 :   mrs_msgs::HwApiPositionCmd cmd;
+    1764             : 
+    1765         264 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1766         264 :   cmd.header.stamp    = ros::Time::now();
+    1767             : 
+    1768         264 :   cmd.position = tracker_command.position;
+    1769         264 :   cmd.heading  = tracker_command.heading;
+    1770             : 
+    1771         264 :   last_control_output_.control_output = cmd;
+    1772             : 
+    1773             :   // fill the unbiased desired accelerations
+    1774         264 :   last_control_output_.desired_unbiased_acceleration = {};
+    1775         264 :   last_control_output_.desired_orientation           = {};
+    1776         264 :   last_control_output_.desired_heading_rate          = {};
+    1777             : 
+    1778             :   // | ----------------- fill in the diagnostics ---------------- |
+    1779             : 
+    1780         264 :   last_control_output_.diagnostics.ramping_up = false;
+    1781             : 
+    1782         264 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1783         264 :   last_control_output_.diagnostics.mass_difference = 0;
+    1784             : 
+    1785         264 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1786             : 
+    1787         264 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1788         264 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1789             : 
+    1790         264 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1791         264 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1792             : 
+    1793         264 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1794         264 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1795             : 
+    1796         264 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1797             : 
+    1798         264 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1799         264 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1800             : 
+    1801         264 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1802         264 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1803             : 
+    1804         264 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1805         264 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1806             : 
+    1807         264 :   last_control_output_.diagnostics.controller = name_;
+    1808             : }
+    1809             : 
+    1810             : //}
+    1811             : 
+    1812             : /* PIDVelocityOutput() //{ */
+    1813             : 
+    1814         705 : void MpcController::PIDVelocityOutput(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command,
+    1815             :                                       const common::CONTROL_OUTPUT &control_output, const double &dt) {
+    1816             : 
+    1817         705 :   if (!tracker_command.use_position_vertical || !tracker_command.use_position_horizontal || !tracker_command.use_heading) {
+    1818           0 :     ROS_ERROR("[%s]: the tracker did not provide position+hdg reference", name_.c_str());
+    1819           0 :     return;
+    1820             :   }
+    1821             : 
+    1822         705 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1823             : 
+    1824         705 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1825         705 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1826             : 
+    1827         705 :   double hdg_ref = tracker_command.heading;
+    1828         705 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1829             : 
+    1830             :   // | ------------------ velocity feedforward ------------------ |
+    1831             : 
+    1832         705 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1833             : 
+    1834         705 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1835         705 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1836             :   }
+    1837             : 
+    1838             :   // | --------------------- control errors --------------------- |
+    1839             : 
+    1840         705 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1841             : 
+    1842             :   // | --------------------------- pid -------------------------- |
+    1843             : 
+    1844         705 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1845         705 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1846         705 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1847             : 
+    1848         705 :   double des_vel_x = position_pid_x_.update(Ep(0), dt);
+    1849         705 :   double des_vel_y = position_pid_y_.update(Ep(1), dt);
+    1850         705 :   double des_vel_z = position_pid_z_.update(Ep(2), dt);
+    1851             : 
+    1852             :   // | -------------------- position feedback ------------------- |
+    1853             : 
+    1854         705 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1855             : 
+    1856         705 :   if (control_output == common::VELOCITY_HDG) {
+    1857             : 
+    1858             :     // | --------------------- fill the output -------------------- |
+    1859             : 
+    1860         458 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1861             : 
+    1862         229 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1863         229 :     cmd.header.stamp    = ros::Time::now();
+    1864             : 
+    1865         229 :     cmd.velocity.x = des_vel(0);
+    1866         229 :     cmd.velocity.y = des_vel(1);
+    1867         229 :     cmd.velocity.z = des_vel(2);
+    1868             : 
+    1869         229 :     cmd.heading = tracker_command.heading;
+    1870             : 
+    1871         229 :     last_control_output_.control_output = cmd;
+    1872             : 
+    1873         476 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1874             : 
+    1875         476 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1876             : 
+    1877         476 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1878             : 
+    1879         476 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1880             : 
+    1881             :     // | --------------------------- ff --------------------------- |
+    1882             : 
+    1883         476 :     double des_hdg_ff = 0;
+    1884             : 
+    1885         476 :     if (tracker_command.use_heading_rate) {
+    1886         476 :       des_hdg_ff = tracker_command.heading_rate;
+    1887             :     }
+    1888             : 
+    1889             :     // | --------------------- fill the output -------------------- |
+    1890             : 
+    1891         952 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1892             : 
+    1893         476 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1894         476 :     cmd.header.stamp    = ros::Time::now();
+    1895             : 
+    1896         476 :     cmd.velocity.x = des_vel(0);
+    1897         476 :     cmd.velocity.y = des_vel(1);
+    1898         476 :     cmd.velocity.z = des_vel(2);
+    1899             : 
+    1900         476 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1901             : 
+    1902         476 :     last_control_output_.control_output = cmd;
+    1903             :   } else {
+    1904             : 
+    1905           0 :     ROS_ERROR("[%s]: the required output of the position PID is not supported", name_.c_str());
+    1906           0 :     return;
+    1907             :   }
+    1908             : 
+    1909             :   // fill the unbiased desired accelerations
+    1910         705 :   last_control_output_.desired_unbiased_acceleration = {};
+    1911         705 :   last_control_output_.desired_orientation           = {};
+    1912         705 :   last_control_output_.desired_heading_rate          = {};
+    1913             : 
+    1914             :   // | ----------------- fill in the diagnostics ---------------- |
+    1915             : 
+    1916         705 :   last_control_output_.diagnostics.ramping_up = false;
+    1917             : 
+    1918         705 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1919         705 :   last_control_output_.diagnostics.mass_difference = 0;
+    1920             : 
+    1921         705 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1922             : 
+    1923         705 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1924         705 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1925             : 
+    1926         705 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1927         705 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1928             : 
+    1929         705 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1930         705 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1931             : 
+    1932         705 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1933             : 
+    1934         705 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1935         705 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1936             : 
+    1937         705 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1938         705 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1939             : 
+    1940         705 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1941         705 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1942             : 
+    1943         705 :   last_control_output_.diagnostics.controller = name_;
+    1944             : }
+    1945             : 
+    1946             : //}
+    1947             : 
+    1948             : // --------------------------------------------------------------
+    1949             : // |                          callbacks                         |
+    1950             : // --------------------------------------------------------------
+    1951             : 
+    1952             : /* //{ callbackDrs() */
+    1953             : 
+    1954         188 : void MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig &config, [[maybe_unused]] uint32_t level) {
+    1955             : 
+    1956         188 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1957             : 
+    1958         188 :   ROS_INFO("[%s]: DRS updated gains", this->name_.c_str());
+    1959         188 : }
+    1960             : 
+    1961             : //}
+    1962             : 
+    1963             : /* //{ callbackSetIntegralTerms() */
+    1964             : 
+    1965           0 : bool MpcController::callbackSetIntegralTerms(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
+    1966             : 
+    1967           0 :   if (!is_initialized_)
+    1968           0 :     return false;
+    1969             : 
+    1970           0 :   integral_terms_enabled_ = req.data;
+    1971             : 
+    1972           0 :   std::stringstream ss;
+    1973             : 
+    1974           0 :   ss << "integral terms %s" << (integral_terms_enabled_ ? "enabled" : "disabled");
+    1975             : 
+    1976           0 :   ROS_INFO_STREAM_THROTTLE(1.0, "[" << name_.c_str() << "]: " << ss.str());
+    1977             : 
+    1978           0 :   res.message = ss.str();
+    1979           0 :   res.success = true;
+    1980             : 
+    1981           0 :   return true;
+    1982             : }
+    1983             : 
+    1984             : //}
+    1985             : 
+    1986             : // --------------------------------------------------------------
+    1987             : // |                           timers                           |
+    1988             : // --------------------------------------------------------------
+    1989             : 
+    1990             : /* timerGains() //{ */
+    1991             : 
+    1992       14508 : void MpcController::timerGains(const ros::TimerEvent &event) {
+    1993             : 
+    1994       29016 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1995       29016 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcController::timerGains", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1996             : 
+    1997       14508 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1998       14508 :   auto gains      = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1999             : 
+    2000             :   // When muting the gains, we want to bypass the filter,
+    2001             :   // so it happens immediately.
+    2002       14508 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    2003       14508 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    2004             : 
+    2005       14508 :   mute_gains_ = false;
+    2006             : 
+    2007       14508 :   double dt = (event.current_real - event.last_real).toSec();
+    2008             : 
+    2009       14508 :   if (!std::isfinite(dt) || (dt <= 0) || (dt > 5 * (1.0 / _gain_filtering_rate_))) {
+    2010         159 :     return;
+    2011             :   }
+    2012             : 
+    2013       14349 :   bool updated = false;
+    2014             : 
+    2015       14349 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    2016       14349 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    2017       14349 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    2018       14349 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    2019       14349 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    2020             : 
+    2021             :   // do not apply muting on these gains
+    2022       14349 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    2023       14349 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    2024       14349 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    2025             : 
+    2026       14349 :   mrs_lib::set_mutexed(mutex_gains_, gains, gains_);
+    2027             : 
+    2028             :   // set the gains back to dynamic reconfigure
+    2029             :   // and only do it when some filtering occurs
+    2030       14349 :   if (updated) {
+    2031             : 
+    2032           0 :     drs_params.kiwxy         = gains.kiwxy;
+    2033           0 :     drs_params.kibxy         = gains.kibxy;
+    2034           0 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    2035           0 :     drs_params.kq_yaw        = gains.kq_yaw;
+    2036           0 :     drs_params.km            = gains.km;
+    2037           0 :     drs_params.km_lim        = gains.km_lim;
+    2038           0 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    2039           0 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    2040             : 
+    2041           0 :     drs_->updateConfig(drs_params);
+    2042             : 
+    2043           0 :     ROS_INFO_THROTTLE(10.0, "[%s]: gains have been updated", name_.c_str());
+    2044             :   }
+    2045             : }
+    2046             : 
+    2047             : //}
+    2048             : 
+    2049             : // --------------------------------------------------------------
+    2050             : // |                       other routines                       |
+    2051             : // --------------------------------------------------------------
+    2052             : 
+    2053             : /* calculateGainChange() //{ */
+    2054             : 
+    2055      114792 : double MpcController::calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name,
+    2056             :                                           bool &updated) {
+    2057             : 
+    2058      114792 :   double change = desired_value - current_value;
+    2059             : 
+    2060      114792 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    2061      114792 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    2062             : 
+    2063      114792 :   if (!bypass_rate) {
+    2064             : 
+    2065             :     // if current value is near 0...
+    2066             :     double change_in_perc;
+    2067             :     double saturated_change;
+    2068             : 
+    2069      114792 :     if (std::abs(current_value) < 1e-6) {
+    2070           0 :       change *= gains_filter_max_change;
+    2071             :     } else {
+    2072             : 
+    2073      114792 :       saturated_change = change;
+    2074             : 
+    2075      114792 :       change_in_perc = ((current_value + saturated_change) / current_value) - 1.0;
+    2076             : 
+    2077      114792 :       if (change_in_perc > gains_filter_max_change) {
+    2078           0 :         saturated_change = current_value * gains_filter_max_change;
+    2079      114792 :       } else if (change_in_perc < -gains_filter_max_change) {
+    2080           0 :         saturated_change = current_value * -gains_filter_max_change;
+    2081             :       }
+    2082             : 
+    2083      114792 :       if (std::abs(saturated_change) < std::abs(change) * gains_filter_min_change) {
+    2084           0 :         change *= gains_filter_min_change;
+    2085             :       } else {
+    2086      114792 :         change = saturated_change;
+    2087             :       }
+    2088             :     }
+    2089             :   }
+    2090             : 
+    2091      114792 :   if (std::abs(change) > 1e-3) {
+    2092           0 :     ROS_DEBUG("[%s]: changing gain '%s' from %.2f to %.2f", name_.c_str(), name.c_str(), current_value, desired_value);
+    2093           0 :     updated = true;
+    2094             :   }
+    2095             : 
+    2096      114792 :   return current_value + change;
+    2097             : }
+    2098             : 
+    2099             : //}
+    2100             : 
+    2101             : /* getHeadingSafely() //{ */
+    2102             : 
+    2103       61398 : double MpcController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    2104             : 
+    2105             :   try {
+    2106       61398 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2107             :   }
+    2108           0 :   catch (...) {
+    2109             :   }
+    2110             : 
+    2111             :   try {
+    2112           0 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYaw();
+    2113             :   }
+    2114           0 :   catch (...) {
+    2115             :   }
+    2116             : 
+    2117           0 :   if (tracker_command.use_heading) {
+    2118           0 :     return tracker_command.heading;
+    2119             :   }
+    2120             : 
+    2121           0 :   return 0;
+    2122             : }
+    2123             : 
+    2124             : //}
+    2125             : 
+    2126             : //}
+    2127             : 
+    2128             : }  // namespace mpc_controller
+    2129             : 
+    2130             : }  // namespace mrs_uav_controllers
+    2131             : 
+    2132             : #include <pluginlib/class_list_macros.h>
+    2133          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_controllers::mpc_controller::MpcController, mrs_uav_managers::Controller)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html new file mode 100644 index 0000000000..8e8f75533d --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html @@ -0,0 +1,554 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/mpc_controller.cpp.gcov.png b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..00a322d92480c1a53fb37119c0044a078e9a1fa8 GIT binary patch literal 6670 zcmV+p8u8_cP)nC0{{R3A004C0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp$_&^9wRXK~0mt-v!NgpL5QQ+d2ZK) zxnR!$#?BNNQWbC=q3LkXJ|UBjjAfct$XZ1eAj{BWGml#P~eMm0~P;Y8j}0 zOqnfxL+3EYAumgH&|vkb0~)%%Y8O}w#%c?^ZUdShg=InCUa^#PC{rR+4Z!`yFcp-E zS*}3moG?@26qJ8h(R=pkTIrW1zzgjH{Sp6RCGXpVzb5Gstdc(NW9eDI&whp@wI}HM zu0^wo5Mx}T+??F+CEJqkGYkp#i9tK3DPX!r*;wWMM65u@^( zfh|D)Tof4BQ`{&qg1KHw5$m1nCDH6RXyM1f`Xx|+#RHNZ<=I#&4G zOm6NPJ|ewnAV&BUE>-cA8=2I9s1gye-p;+6)EJfnHO%X7( zM5mC)h4pb+y3Txz&bsReK3=IB;Jk(51s^I2C>8LvI)o1rtPbcQsq z5)}wNL)QhY=54@rfXy7-g+a2axPaU&Rkz}JKy?F;#|U(n5HHZkGkL72KF@=~e_R+a z>+ttyW{EmBE=Ix_K|hPNA7$4(ASQ7!6UlU))|Ccer9~rd5?}{0EkNiztik^%2{f+r z4+M2yU9t3@w)U!kr*Oh0fgSf|NGdO?lK}L~_;0DFH1r1GmH^p-Si5$ZEey^b@aJPk`B#8;P%RT|#2A%*^bjn` zYaA?Xv*dy?N|NI^G1F2_@noJ_F!Dol6!7coJG96C|Kt1l^WV|JukYvA-aOzVzLneM zs|67O^$o7qYn-|>aRrg#q`ef-AU*!ckIz4RAhS|1VDv_M+K$RQ(kd}v7&8D3?0Ff+ z@qjNJ!Hw=P@ooZeSKfC&Mpk+TML;h@a{W$4EAAZ#l+m zQ@me0Mu2g*n+iK>OMrTe#17xeJ?hT~4%^{Hd+dhy@!u=$@V_Ov;{h#Hr?jNn6EUf4 zP4@HT_1f!Hloaz8>-BJ|f5#X_57-^L3>3}+?=N^0XF&|zpUl1H*pt<82eE=>elPA60asyFB^=9+Sh`Eh-3jLHTX zz9ij!^5F{Y5RDDICSBVBDU&u45MX3{&I!=J-2|{6jDA$I-`)VIYzbp}0v#VODA$=5 z#+}=&OzsBm&wqrN+8DLOls>v^xAdLPIyfN*f6?UpfCa|VzVAl-DVw6 z9~clXBjOJII$qit4!%qRln~H@(P4Z7pdO=gKLgEmS(_e;G*Sssvtvwv$3J(Ezg_(3 zczd}i1yVoP&A{TJZpQKPzzC_XanLi%0;nD1tGjN`W84?IJ6ME>KpZxbR|4KM)K)Io z?x8>8_otMJiFxtx085cFeZ$H}+^*Q2S52CL;5yXp*1d88Emhli^HpLpIW$7&>9F1-N z(+`)LfHy6mA888^1kwLvWNfdAPRtJCYmH|9dcADd9gD z_k=A4;JjyMf%!EcRkJ4wK)kbj>vAi$o*DBLhE$YC%t(2p8#1rko>o3YuNV z*R`&nU<7Q}tY|d5W)gu)s&mJduID5IA^8y&9h(QqHLyjDgonJZD*&pxp5ZN|;WQmF zSsH+d+~0{9Q9-MSxgT+^HzKAYxCIrM4g~lf#0YbvASQ36-hvpN0^o=#0AEi`#iVJ@ z%!UC!#C(VW&07GE_QL*kKq@SNFQu(8A&5+D3Yk5>Dy}f>=9FR?g>Qfucc<-8!9I(z zlDE^B2sFmwtDnND6 zoCQ$X^?nyq^$s?3mWw?O)>}ks`0KzeA-6W0cOEwa#cDz%)7m&@#q~p^yaX*KGl~#uZ__#5#=dletA0uS2i= zB-@m%0$lvb9Zti#0=owFU8gJ(*4T9E$;mNI7BHXN41hPL#-WF@OE^BVycA~^`}Q*| zH6&Kjd6W#D^F2StgLRalx$Dvgz#GPlrl3=_34nAiH-a&%7T&7?F>VNuz@9&7Q&elz zW5g$$3i0ItZyaD`tU-FzLQ$NxWz)ZI1W+~t{6QNjA+4`yiH(@sjbS1hZzhK4_GWkE z%(@ENuykF4CX4RS;}27pt~KYiEFsn9-vDYGI5mh>P$9VE0kzU|qnQfAI|HCvaO3?L zYrbW`7ar~P?d2(K&7KzU#u8il=OsY;-r3j_tD00FkerNDD+#jMEP!>5^>b?cmZ+u+ zn(>gIMfV&ntDpSYRNVv1pk(D2K0R~Rem`QTIL^@qG%%d8_C{1!_HhLy8hl>!KtKc4 z*W+%CaeE&R+Zo||Bj(z1v-bl#`}uRrdWbMuzj&< zcG2DU_y*;+a$_AyF%@{c&BZgEvfHIxg=TerGs6kHc|M+JcxLyb$?%wdIf>zSoDQ1w z3u5VntoT{JrjIrg0YBR8qs>0r?4!*TeSfr>-1SGBiR^Z_k2V{~@IR%^NG>4SUY{?_ z$9ue;D}7P}0F^D(IA1co5-}5FAC3?rxZ(!_YT4r}-yi>$=mJd_O=K@j1PqrXU*mzm z6GFO&NcPEez+K2`BXq{c#RwyHJm29uGZ1jXS%44K{}Zam=RXeS<0L>WA7{i17r4O1 z3xUoraUim=p^S^f@B(mSD|GkdNxh(J!LX-**oJ#nj;E^uC*;Kad1A^`1T@CV5Wft7 zA7ZL)PHk?=9R6|b0<0A+;hq@wm=+HxCk)W$NXS+Itc}t6LYR1y#sC&cQUv^53hQTP zN{nwYGb}TE0pDR}3U9=(m>F0RDE$K$D37@hslI&FmISnXGyx|sA4|$T8nb+``Ne6j zwNnTVr@MM}TFn(-oj-j}J8xGj03OvD4iSMBB@;U)d6S?h{0$|IvKma_EiL5S#0A{CMllmqft;UHLAn0ly4AF z$8WMK6qq33lwy|te9?vpx#LHy^N?Re2*fkhiR;C9m)0*mduE-C&-8h+!K<0kJ$rI- zw4TwmD)Z0e#WdUz2Y7o@s$=iS3YbXs2}6?=_6~VOh4@Mv+8EutW^2sM45ktJtFp%d z^&~t)Vp0P~by2PpeI@kdPhWWj)?&C&Xw=2?p<6N)R4U*yKj))*nGS38xDxP|RC-eN zOmfJN74SR^2c|uN6>(%}_{tmKF-7EO16|q~2S(+#d6M<9HqZ0W0*B$YGLGS@5|to# zra?Pv#KoE>G0WvhEduu;i2eX0L3&NNt& zVjs6;r&s`$U8hSHlY4Ikmsg=_8*lfp{;*T-Fl6l2++nzD`JM^@7`q=cY6QIgJ6D(4)A|Zd6SA;LONQXXLx6d=v05nkjd1l`AnYpHX?jh*^ zFAj~(zt~^>d54C7K{0-vaA^1!RKj2XC>;C3YLYQ^XjJs$uiF=(0w5&qMPm-n(5GJ5d(*RuT?Jo2=4z~aEV9bZ#gvJ&K^!SUFmvK z{0X;-Nn%ZtU{I3hp?CKY8NdVDF;?!1m;`7WBR=3a0wZE1!%2%{X!OyhTeh#OX0Cb9 zMakwd!ugYUZn2t8RJUdPZ%Vk_wghOwSf(`7)`O8giE)xPvwqC3;V?)M(2NlcjiwGx z(GVA0xq7xdeuEUAqJZe=JADCjc6# zZl-;j?sn;M*+DlBefdhK3p~9Ks(f?uC^HZGer+)%P-#5-xVtNj3u7#Hh#eue0w;Es zRM>+Pm&GyCV%`JB80Tgn#tj`Gwg3+5dg}oE%~WH|<1ohB-Yviz031b3K-mPmWx!Eg zhZ62h!2O*3W<3iaba+blDAt8ZS;!-EBraVUzA3>M#bMqv!}nw=M|ZGE777?bs&&2p zU<7x6_PkRm_-vwW)`~S1vEjrf7o$y^|75~mL5K|1%b);XX_47vYVAkcJ8=5znXz`H6fe8l$xtniA1)3-oTmqFL{JcSBp zn$f(5y1wFs=n#tMGv)R`CI8I%J^7O?Ga<4Dk|z*gjEiz;3C0+vdw_V|BLeOXSNTNm zvOGeJ0M`Qzg~b>K@$8hxqicnfPrTl7V+6NPC^`&wa1k5Si2vl4lY!U3c|O*4Zs+4# zd1#G$g}4eOikGx(L`AsEV^IYeiFoV)tArEYIaZ4 z8XrM7JcBlSM0NGjT&@{~B9p4{vKjsnf$s|MS68KD&$Sh>pL;D%Vpa+7=7#?^Qg z`)yoNKlk`4%Jf?B`^&KBQnm}1JbUL|$W$EVW7Wme8mma)#}#!gOYkM#xzV<9OAD7~ za{sj1HGFF;wlGFy)g8ZCz?gvzxA}0cSyb`eJ7T0bD;R^OVqA72w%t$i4;fCqD+Abr zw~G82d15B|9|z^7A@f=)c;{8hctBq}!q+(8WBmCJUe1yDsuhWVlj=WeG3H1Y;QPM;e23dP z78r|LyKUT@)tF;yDLMq<*sLAqxwSdYH3^kswQt&gM@;iND4|jq<5qYUOaU<-C&*N+ z0@5?>(imw%<$y89-UnjD8G{zUL0xYhfWMh)f_WImm?tUK+>h-B07ns%b2RI4J;|Jn zu0uj)v+I~pdAjS;W6_}>*X;I9xg7)aqkMR$hw9r+tg2G5r($VBz)h^TZ5$(Wo|8M< ztjEZoF7jjU$N0MJ*=pme+4^+X1)B=In%hy30PF-fNp>u-(G!~ zwtZpB;z}2aatwz%@k$P@scW6Wx|Qb(*{0tDC`0ThqmrZx<=nGxTehNF!eL~{Nn6~x zao05^Xsqzv`3rT?Z53|7<4T_jmjTrnnXK=0RTS7UbYN_kH`>B1$KP#?^7DYtruc70 zKoVEH&yCu{GH_`dH$Zwwi=X$UHS%0EN5tjQ9vJY2S$1(|Q2JF)JnivQ=)YlC_GpUf z+)q*VFOQO9!#*c30sXKfhym|FtI|A~VkCAZ3hV+>-#xk<`Af1)od-xFNtHT|;~) zORJ3uh}Fj!*`op6I6wn2|1QMV$#7-Ol4^*p*Xt@yALPvq=e z*Y6NH`(>8YpkgGfB~=gYPKa>d`^=G?9ddB(!!qm zH~?X6Hh1Ugjey#&p-ll!Xbm~WdM5z_N{pU8{2bFyL!68EyEp_m_qbcMHP`&f89uro z8)q+}#$&vuYg^BcQD|@q#{j0;v_D*BO$8d>9}n0yGwCs02h0qk&A|CQXX0bZg-W=(}}{%=)pSw|$SR3q!0Eqo=xHAQ#w3p~^;e^_w9zUEgM_ zJxTF>)LskFI>vPE*2WS(L+$C#7aQcUJnxz+k?fzuRH^5SvmZWwT)BPxxP2J^pU%{e zA9rNeA3v@Vqh8lfjfiti$db6AzwK>mJ-LW;P}OT8YELHgv`}pK-b~ zF;jOvMa-~6t$vlDqS2ZIOiM+^bS4+5qF1%jytVge8+?|o!Kk0|2s$u6o5ZLwKAwrI zPsIH&U-dc973iY!-4rVopF}AzaxgY&U{K^zokpOt(0(S1+wQ{AR96r+HH*)dd zKbgZ77HAP?=C<32h=1f3Ovvz1#DoQm+_z3)ts<9|J;xZqwBQ*+w->1~W*XCJn;%;D z$QVTa%6!~g_TmmI6e&33U_F~awSTvNi2=;MIS=Tu7l5!_P813OV?Ix40Mt@_Nww*{ zmwA<*n`A$-%*g^{2CQ?@nCnWL3#cfdfjwUD1g#TP3&S6OCe)u3&1X_LKKh7en;7jF zvs-ar=R=ug{^koa;9)(1YVIQ7Gh<|omIUY#ctF-lF=O<9PGc;{u>k7Yr7Y$q0`_1` zb@ZS4JZ%`g-r@TTuKVaT3*%`#@X>s;@Mxn_{KXu9=TFn-3nR;RFV%;>>tN1oaPcX% zquKa?`j!NJbAYxAm_a+Nl*Y{|=41Zxs+Lv0wCQrqL4-QX0pO5*#f_dN6X*|z@^MWC zLH9hR3jY|;Fh;yRIX^Qu8d-z_1Ws++FqWuFX+jqW#Ki+$We_y-juGWQU^9F(VEj%q zqrQ^BFf$~h(Q)IAV{D4gF-DE}95aJ>U74BD_v8*=xd0b;f<$BPf-7S@)2jZ2RZJf# z!?)=`nsulO?uJETepdiulqcuTuGgS4NBC*kR44<_H@a%_ZPV|sm8bmYWELo?j{fj! zds@c$LaLQ_>3o$`i<^oGV=V7TVKM#+U-S6ec@}tY;0T>UF!tA2 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/se3_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - se3_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:62577580.6 %
Date:2024-04-25 05:42:57Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::se3_controller::Se3Controller::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::se3_controller::Se3Controller::resetDisturbanceEstimators()0
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()24
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)27
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::se3_controller::Se3Controller::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)189
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)226
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)372
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()2868
mrs_uav_controllers::se3_controller::Se3Controller::PIDVelocityOutput(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&, double const&)2920
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4566
mrs_uav_controllers::se3_controller::Se3Controller::SE3Controller(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&)25344
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28264
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28490
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)63546
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)108365
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.func.html b/mrs_uav_controllers/src/se3_controller.cpp.func.html new file mode 100644 index 0000000000..d638eeeeff --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.func.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/se3_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - se3_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:62577580.6 %
Date:2024-04-25 05:42:57Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()24
mrs_uav_controllers::se3_controller::Se3Controller::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4566
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)189
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28490
mrs_uav_controllers::se3_controller::Se3Controller::SE3Controller(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&)25344
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)372
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)108365
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28264
mrs_uav_controllers::se3_controller::Se3Controller::PIDVelocityOutput(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&, double const&)2920
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)63546
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)226
mrs_uav_controllers::se3_controller::Se3Controller::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::se3_controller::Se3Controller::resetDisturbanceEstimators()0
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)27
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()2868
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.gcov.frameset.html b/mrs_uav_controllers/src/se3_controller.cpp.gcov.frameset.html new file mode 100644 index 0000000000..96cbbf6f55 --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/se3_controller.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.gcov.html b/mrs_uav_controllers/src/se3_controller.cpp.gcov.html new file mode 100644 index 0000000000..e3fe2253f8 --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.gcov.html @@ -0,0 +1,1930 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/se3_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - se3_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:62577580.6 %
Date:2024-04-25 05:42:57Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <common.h>
+       7             : #include <pid.hpp>
+       8             : 
+       9             : #include <mrs_uav_managers/controller.h>
+      10             : 
+      11             : #include <dynamic_reconfigure/server.h>
+      12             : #include <mrs_uav_controllers/se3_controllerConfig.h>
+      13             : 
+      14             : #include <mrs_lib/profiler.h>
+      15             : #include <mrs_lib/utils.h>
+      16             : #include <mrs_lib/mutex.h>
+      17             : #include <mrs_lib/attitude_converter.h>
+      18             : #include <mrs_lib/geometry/cyclic.h>
+      19             : 
+      20             : #include <geometry_msgs/Vector3Stamped.h>
+      21             : 
+      22             : //}
+      23             : 
+      24             : #define OUTPUT_ACTUATORS 0
+      25             : #define OUTPUT_CONTROL_GROUP 1
+      26             : #define OUTPUT_ATTITUDE_RATE 2
+      27             : #define OUTPUT_ATTITUDE 3
+      28             : 
+      29             : namespace mrs_uav_controllers
+      30             : {
+      31             : 
+      32             : namespace se3_controller
+      33             : {
+      34             : 
+      35             : /* structs //{ */
+      36             : 
+      37             : typedef struct
+      38             : {
+      39             :   double kpxy;           // position xy gain
+      40             :   double kvxy;           // velocity xy gain
+      41             :   double kaxy;           // acceleration xy gain (feed forward, =1)
+      42             :   double kiwxy;          // world xy integral gain
+      43             :   double kibxy;          // body xy integral gain
+      44             :   double kiwxy_lim;      // world xy integral limit
+      45             :   double kibxy_lim;      // body xy integral limit
+      46             :   double kpz;            // position z gain
+      47             :   double kvz;            // velocity z gain
+      48             :   double kaz;            // acceleration z gain (feed forward, =1)
+      49             :   double km;             // mass estimator gain
+      50             :   double km_lim;         // mass estimator limit
+      51             :   double kq_roll_pitch;  // pitch/roll attitude gain
+      52             :   double kq_yaw;         // yaw attitude gain
+      53             :   double kw_roll_pitch;  // attitude rate gain
+      54             :   double kw_yaw;         // attitude rate gain
+      55             : } Gains_t;
+      56             : 
+      57             : //}
+      58             : 
+      59             : /* //{ class Se3Controller */
+      60             : 
+      61             : class Se3Controller : public mrs_uav_managers::Controller {
+      62             : 
+      63             : public:
+      64             :   bool initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      65             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      66             : 
+      67             :   bool activate(const ControlOutput& last_control_output);
+      68             : 
+      69             :   void deactivate(void);
+      70             : 
+      71             :   void updateInactive(const mrs_msgs::UavState& uav_state, const std::optional<mrs_msgs::TrackerCommand>& tracker_command);
+      72             : 
+      73             :   ControlOutput updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command);
+      74             : 
+      75             :   const mrs_msgs::ControllerStatus getStatus();
+      76             : 
+      77             :   void switchOdometrySource(const mrs_msgs::UavState& new_uav_state);
+      78             : 
+      79             :   void resetDisturbanceEstimators(void);
+      80             : 
+      81             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd);
+      82             : 
+      83             : private:
+      84             :   ros::NodeHandle nh_;
+      85             : 
+      86             :   bool is_initialized_ = false;
+      87             :   bool is_active_      = false;
+      88             : 
+      89             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      90             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      91             : 
+      92             :   // | ------------------------ uav state ----------------------- |
+      93             : 
+      94             :   mrs_msgs::UavState uav_state_;
+      95             :   std::mutex         mutex_uav_state_;
+      96             : 
+      97             :   // | --------------- dynamic reconfigure server --------------- |
+      98             : 
+      99             :   boost::recursive_mutex                            mutex_drs_;
+     100             :   typedef mrs_uav_controllers::se3_controllerConfig DrsConfig_t;
+     101             :   typedef dynamic_reconfigure::Server<DrsConfig_t>  Drs_t;
+     102             :   boost::shared_ptr<Drs_t>                          drs_;
+     103             :   void                                              callbackDrs(mrs_uav_controllers::se3_controllerConfig& config, uint32_t level);
+     104             :   DrsConfig_t                                       drs_params_;
+     105             : 
+     106             :   // | ----------------------- controllers ---------------------- |
+     107             : 
+     108             :   void positionPassthrough(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command);
+     109             : 
+     110             :   void PIDVelocityOutput(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command, const common::CONTROL_OUTPUT& control_output,
+     111             :                          const double& dt);
+     112             : 
+     113             :   void SE3Controller(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command, const double& dt,
+     114             :                      const common::CONTROL_OUTPUT& output_modality);
+     115             : 
+     116             :   // | ----------------------- constraints ---------------------- |
+     117             : 
+     118             :   mrs_msgs::DynamicsConstraints constraints_;
+     119             :   std::mutex                    mutex_constraints_;
+     120             : 
+     121             :   // | --------- throttle generation and mass estimation -------- |
+     122             : 
+     123             :   double _uav_mass_;
+     124             :   double uav_mass_difference_;
+     125             : 
+     126             :   Gains_t gains_;
+     127             : 
+     128             :   std::mutex mutex_gains_;       // locks the gains the are used and filtered
+     129             :   std::mutex mutex_drs_params_;  // locks the gains that came from the drs
+     130             : 
+     131             :   ros::Timer timer_gains_;
+     132             :   void       timerGains(const ros::TimerEvent& event);
+     133             : 
+     134             :   double _gain_filtering_rate_;
+     135             : 
+     136             :   // | ----------------------- gain muting ---------------------- |
+     137             : 
+     138             :   std::atomic<bool> mute_gains_            = false;
+     139             :   std::atomic<bool> mute_gains_by_tracker_ = false;
+     140             :   double            _gain_mute_coefficient_;
+     141             : 
+     142             :   // | --------------------- gain filtering --------------------- |
+     143             : 
+     144             :   double calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name, bool& updated);
+     145             : 
+     146             :   double getHeadingSafely(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command);
+     147             : 
+     148             :   double _gains_filter_change_rate_;
+     149             :   double _gains_filter_min_change_rate_;
+     150             : 
+     151             :   // | ------------ controller limits and saturations ----------- |
+     152             : 
+     153             :   bool   _tilt_angle_failsafe_enabled_;
+     154             :   double _tilt_angle_failsafe_;
+     155             : 
+     156             :   double _throttle_saturation_;
+     157             : 
+     158             :   // | ------------------ activation and output ----------------- |
+     159             : 
+     160             :   ControlOutput last_control_output_;
+     161             :   ControlOutput activation_control_output_;
+     162             : 
+     163             :   ros::Time         last_update_time_;
+     164             :   std::atomic<bool> first_iteration_ = true;
+     165             : 
+     166             :   // | ------------------------ profiler_ ------------------------ |
+     167             : 
+     168             :   mrs_lib::Profiler profiler_;
+     169             :   bool              _profiler_enabled_ = false;
+     170             : 
+     171             :   // | ------------------------ integrals ----------------------- |
+     172             : 
+     173             :   Eigen::Vector2d Ib_b_;  // body error integral in the body frame
+     174             :   Eigen::Vector2d Iw_w_;  // world error integral in the world_frame
+     175             :   std::mutex      mutex_integrals_;
+     176             : 
+     177             :   // | ------------------------- rampup ------------------------- |
+     178             : 
+     179             :   bool   _rampup_enabled_ = false;
+     180             :   double _rampup_speed_;
+     181             : 
+     182             :   bool      rampup_active_ = false;
+     183             :   double    rampup_throttle_;
+     184             :   int       rampup_direction_;
+     185             :   double    rampup_duration_;
+     186             :   ros::Time rampup_start_time_;
+     187             :   ros::Time rampup_last_time_;
+     188             : 
+     189             :   // | ---------------------- position pid ---------------------- |
+     190             : 
+     191             :   double _pos_pid_p_;
+     192             :   double _pos_pid_i_;
+     193             :   double _pos_pid_d_;
+     194             : 
+     195             :   double _hdg_pid_p_;
+     196             :   double _hdg_pid_i_;
+     197             :   double _hdg_pid_d_;
+     198             : 
+     199             :   PIDController position_pid_x_;
+     200             :   PIDController position_pid_y_;
+     201             :   PIDController position_pid_z_;
+     202             :   PIDController position_pid_heading_;
+     203             : };
+     204             : 
+     205             : //}
+     206             : 
+     207             : // --------------------------------------------------------------
+     208             : // |                   controller's interface                   |
+     209             : // --------------------------------------------------------------
+     210             : 
+     211             : /* //{ initialize() */
+     212             : 
+     213          94 : bool Se3Controller::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     214             :                                std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     215             : 
+     216          94 :   nh_ = nh;
+     217             : 
+     218          94 :   common_handlers_  = common_handlers;
+     219          94 :   private_handlers_ = private_handlers;
+     220             : 
+     221          94 :   _uav_mass_ = common_handlers->getMass();
+     222             : 
+     223          94 :   ros::Time::waitForValid();
+     224             : 
+     225             :   // | ---------- loading params using the parent's nh ---------- |
+     226             : 
+     227         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     228             : 
+     229          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     230             : 
+     231          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     232           0 :     ROS_ERROR("[MidairActivationController]: Could not load all parameters!");
+     233           0 :     return false;
+     234             :   }
+     235             : 
+     236             :   // | -------------------- loading my params ------------------- |
+     237             : 
+     238          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/se3_controller.yaml");
+     239          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/se3_controller.yaml");
+     240             : 
+     241         188 :   const std::string yaml_namespace = "mrs_uav_controllers/se3_controller/";
+     242             : 
+     243             :   // lateral gains
+     244          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kp", gains_.kpxy);
+     245          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kv", gains_.kvxy);
+     246          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/ka", gains_.kaxy);
+     247             : 
+     248          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw", gains_.kiwxy);
+     249          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib", gains_.kibxy);
+     250             : 
+     251             :   // | ------------------------- rampup ------------------------- |
+     252             : 
+     253          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/enabled", _rampup_enabled_);
+     254          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/speed", _rampup_speed_);
+     255             : 
+     256             :   // height gains
+     257          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kp", gains_.kpz);
+     258          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kv", gains_.kvz);
+     259          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/ka", gains_.kaz);
+     260             : 
+     261             :   // attitude gains
+     262          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_roll_pitch", gains_.kq_roll_pitch);
+     263          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_yaw", gains_.kq_yaw);
+     264             : 
+     265             :   // attitude rate gains
+     266          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_roll_pitch", gains_.kw_roll_pitch);
+     267          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_yaw", gains_.kw_yaw);
+     268             : 
+     269             :   // mass estimator
+     270          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km", gains_.km);
+     271          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km_lim", gains_.km_lim);
+     272             : 
+     273             :   // integrator limits
+     274          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw_lim", gains_.kiwxy_lim);
+     275          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib_lim", gains_.kibxy_lim);
+     276             : 
+     277             :   // constraints
+     278          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     279          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     280             : 
+     281          94 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     282             : 
+     283          94 :   if (_tilt_angle_failsafe_enabled_ && std::abs(_tilt_angle_failsafe_) < 1e-3) {
+     284           0 :     ROS_ERROR("[Se3Controller]: constraints/tilt_angle_failsafe/enabled = 'TRUE' but the limit is too low");
+     285           0 :     return false;
+     286             :   }
+     287             : 
+     288          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/throttle_saturation", _throttle_saturation_);
+     289             : 
+     290             :   // gain filtering
+     291          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     292          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     293          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/rate", _gain_filtering_rate_);
+     294          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     295             : 
+     296             :   // output mode
+     297          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/preferred_output", drs_params_.preferred_output_mode);
+     298             : 
+     299          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rotation_matrix", drs_params_.rotation_type);
+     300             : 
+     301             :   // angular rate feed forward
+     302         188 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/parasitic_pitch_roll",
+     303          94 :                                             drs_params_.pitch_roll_heading_rate_compensation);
+     304          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     305             : 
+     306             :   // | ------------------- position pid params ------------------ |
+     307             : 
+     308          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     309          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     310          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     311             : 
+     312          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     313          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     314          94 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     315             : 
+     316             :   // | ------------------ finish loading params ----------------- |
+     317             : 
+     318          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     319           0 :     ROS_ERROR("[Se3Controller]: could not load all parameters!");
+     320           0 :     return false;
+     321             :   }
+     322             : 
+     323             :   // | ---------------- prepare stuff from params --------------- |
+     324             : 
+     325          94 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     326          94 :         drs_params_.preferred_output_mode == OUTPUT_ATTITUDE_RATE || drs_params_.preferred_output_mode == OUTPUT_ATTITUDE)) {
+     327           0 :     ROS_ERROR("[Se3Controller]: preferred output mode has to be {0, 1, 2, 3}!");
+     328           0 :     return false;
+     329             :   }
+     330             : 
+     331             :   // initialize the integrals
+     332          94 :   uav_mass_difference_ = 0;
+     333          94 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     334          94 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     335             : 
+     336             :   // | --------------- dynamic reconfigure server --------------- |
+     337             : 
+     338          94 :   drs_params_.kpxy             = gains_.kpxy;
+     339          94 :   drs_params_.kvxy             = gains_.kvxy;
+     340          94 :   drs_params_.kaxy             = gains_.kaxy;
+     341          94 :   drs_params_.kiwxy            = gains_.kiwxy;
+     342          94 :   drs_params_.kibxy            = gains_.kibxy;
+     343          94 :   drs_params_.kpz              = gains_.kpz;
+     344          94 :   drs_params_.kvz              = gains_.kvz;
+     345          94 :   drs_params_.kaz              = gains_.kaz;
+     346          94 :   drs_params_.kq_roll_pitch    = gains_.kq_roll_pitch;
+     347          94 :   drs_params_.kq_yaw           = gains_.kq_yaw;
+     348          94 :   drs_params_.kiwxy_lim        = gains_.kiwxy_lim;
+     349          94 :   drs_params_.kibxy_lim        = gains_.kibxy_lim;
+     350          94 :   drs_params_.km               = gains_.km;
+     351          94 :   drs_params_.km_lim           = gains_.km_lim;
+     352          94 :   drs_params_.jerk_feedforward = true;
+     353             : 
+     354          94 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     355          94 :   drs_->updateConfig(drs_params_);
+     356          94 :   Drs_t::CallbackType f = boost::bind(&Se3Controller::callbackDrs, this, _1, _2);
+     357          94 :   drs_->setCallback(f);
+     358             : 
+     359             :   // | ------------------------- timers ------------------------- |
+     360             : 
+     361          94 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &Se3Controller::timerGains, this, false, false);
+     362             : 
+     363             :   // | ---------------------- position pid ---------------------- |
+     364             : 
+     365          94 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     366          94 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     367          94 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     368          94 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     369             : 
+     370             :   // | ------------------------ profiler ------------------------ |
+     371             : 
+     372          94 :   profiler_ = mrs_lib::Profiler(common_handlers_->parent_nh, "Se3Controller", _profiler_enabled_);
+     373             : 
+     374             :   // | ----------------------- finish init ---------------------- |
+     375             : 
+     376          94 :   ROS_INFO("[Se3Controller]: initialized");
+     377             : 
+     378          94 :   is_initialized_ = true;
+     379             : 
+     380          94 :   return true;
+     381             : }
+     382             : 
+     383             : //}
+     384             : 
+     385             : /* //{ activate() */
+     386             : 
+     387          27 : bool Se3Controller::activate(const ControlOutput& last_control_output) {
+     388             : 
+     389          27 :   activation_control_output_ = last_control_output;
+     390             : 
+     391          27 :   double activation_mass = _uav_mass_;
+     392             : 
+     393          27 :   if (activation_control_output_.diagnostics.mass_estimator) {
+     394           1 :     uav_mass_difference_ = activation_control_output_.diagnostics.mass_difference;
+     395           1 :     activation_mass += uav_mass_difference_;
+     396           1 :     ROS_INFO("[Se3Controller]: setting mass difference from the last control output: %.2f kg", uav_mass_difference_);
+     397             :   }
+     398             : 
+     399          27 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     400             : 
+     401          27 :   if (activation_control_output_.diagnostics.disturbance_estimator) {
+     402           1 :     Ib_b_(0) = -activation_control_output_.diagnostics.disturbance_bx_b;
+     403           1 :     Ib_b_(1) = -activation_control_output_.diagnostics.disturbance_by_b;
+     404             : 
+     405           1 :     Iw_w_(0) = -activation_control_output_.diagnostics.disturbance_wx_w;
+     406           1 :     Iw_w_(1) = -activation_control_output_.diagnostics.disturbance_wy_w;
+     407             : 
+     408           1 :     ROS_INFO(
+     409             :         "[Se3Controller]: setting disturbances from the last control output: Ib_b_: %.2f, %.2f N, Iw_w_: "
+     410             :         "%.2f, %.2f N",
+     411             :         Ib_b_(0), Ib_b_(1), Iw_w_(0), Iw_w_(1));
+     412             :   }
+     413             : 
+     414             :   // did the last controller use manual throttle control?
+     415          27 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     416             : 
+     417             :   // rampup check
+     418          27 :   if (_rampup_enabled_ && throttle_last_controller) {
+     419             : 
+     420          14 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     421             : 
+     422          14 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     423             : 
+     424          14 :     if (throttle_difference > 0) {
+     425           3 :       rampup_direction_ = 1;
+     426          11 :     } else if (throttle_difference < 0) {
+     427           0 :       rampup_direction_ = -1;
+     428             :     } else {
+     429          11 :       rampup_direction_ = 0;
+     430             :     }
+     431             : 
+     432          14 :     ROS_INFO("[Se3Controller]: activating rampup with initial throttle: %.4f, target: %.4f", throttle_last_controller.value(), hover_throttle);
+     433             : 
+     434          14 :     rampup_active_     = true;
+     435          14 :     rampup_start_time_ = ros::Time::now();
+     436          14 :     rampup_last_time_  = ros::Time::now();
+     437          14 :     rampup_throttle_   = throttle_last_controller.value();
+     438             : 
+     439          14 :     rampup_duration_ = std::abs(throttle_difference) / _rampup_speed_;
+     440             :   }
+     441             : 
+     442          27 :   first_iteration_ = true;
+     443          27 :   mute_gains_      = true;
+     444             : 
+     445          27 :   timer_gains_.start();
+     446             : 
+     447             :   // | ------------------ finish the activation ----------------- |
+     448             : 
+     449          27 :   ROS_INFO("[Se3Controller]: activated");
+     450             : 
+     451          27 :   is_active_ = true;
+     452             : 
+     453          27 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* //{ deactivate() */
+     459             : 
+     460          24 : void Se3Controller::deactivate(void) {
+     461             : 
+     462          24 :   is_active_           = false;
+     463          24 :   first_iteration_     = false;
+     464          24 :   uav_mass_difference_ = 0;
+     465             : 
+     466          24 :   timer_gains_.stop();
+     467             : 
+     468          24 :   ROS_INFO("[Se3Controller]: deactivated");
+     469          24 : }
+     470             : 
+     471             : //}
+     472             : 
+     473             : /* updateInactive() //{ */
+     474             : 
+     475      108365 : void Se3Controller::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command) {
+     476             : 
+     477      108365 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     478             : 
+     479      108365 :   last_update_time_ = uav_state.header.stamp;
+     480             : 
+     481      108365 :   first_iteration_ = false;
+     482      108365 : }
+     483             : 
+     484             : //}
+     485             : 
+     486             : /* //{ updateWhenActive() */
+     487             : 
+     488       28490 : Se3Controller::ControlOutput Se3Controller::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+     489             : 
+     490       85470 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateActive");
+     491       85470 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::updateActive", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     492             : 
+     493       56980 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     494             : 
+     495       28490 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     496             : 
+     497       28490 :   last_control_output_.desired_heading_rate          = {};
+     498       28490 :   last_control_output_.desired_orientation           = {};
+     499       28490 :   last_control_output_.desired_unbiased_acceleration = {};
+     500       28490 :   last_control_output_.control_output                = {};
+     501             : 
+     502             :   // | -------------------- calculate the dt -------------------- |
+     503             : 
+     504             :   double dt;
+     505             : 
+     506       28490 :   if (first_iteration_) {
+     507          27 :     dt               = 0.01;
+     508          27 :     first_iteration_ = false;
+     509             :   } else {
+     510       28463 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     511             :   }
+     512             : 
+     513       28490 :   last_update_time_ = uav_state.header.stamp;
+     514             : 
+     515       28490 :   if (std::abs(dt) < 0.001) {
+     516             : 
+     517           0 :     ROS_DEBUG("[Se3Controller]: the last odometry message came too close (%.2f s)!", dt);
+     518             : 
+     519           0 :     dt = 0.01;
+     520             :   }
+     521             : 
+     522             :   // | ----------- obtain the lowest possible modality ---------- |
+     523             : 
+     524       28490 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     525             : 
+     526       28490 :   if (!lowest_modality) {
+     527             : 
+     528           0 :     ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: output modalities are empty! This error should never appear.");
+     529             : 
+     530           0 :     return last_control_output_;
+     531             :   }
+     532             : 
+     533             :   // | ----- we might prefer some output mode over the other ---- |
+     534             : 
+     535       28490 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     536       20712 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing attitude rate output");
+     537       20712 :     lowest_modality = common::ATTITUDE_RATE;
+     538        7778 :   } else if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE && common_handlers_->control_output_modalities.attitude) {
+     539           0 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing attitude output");
+     540           0 :     lowest_modality = common::ATTITUDE;
+     541        7778 :   } else if (drs_params.preferred_output_mode == OUTPUT_CONTROL_GROUP && common_handlers_->control_output_modalities.control_group) {
+     542           0 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing control group output");
+     543           0 :     lowest_modality = common::CONTROL_GROUP;
+     544        7778 :   } else if (drs_params.preferred_output_mode == OUTPUT_ACTUATORS && common_handlers_->control_output_modalities.actuators) {
+     545           0 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing actuators output");
+     546           0 :     lowest_modality = common::ACTUATORS_CMD;
+     547             :   }
+     548             : 
+     549       28490 :   switch (lowest_modality.value()) {
+     550             : 
+     551         226 :     case common::POSITION: {
+     552         226 :       positionPassthrough(uav_state, tracker_command);
+     553         226 :       break;
+     554             :     }
+     555             : 
+     556        1078 :     case common::VELOCITY_HDG: {
+     557        1078 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     558        1078 :       break;
+     559             :     }
+     560             : 
+     561        1842 :     case common::VELOCITY_HDG_RATE: {
+     562        1842 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     563        1842 :       break;
+     564             :     }
+     565             : 
+     566         551 :     case common::ACCELERATION_HDG: {
+     567         551 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     568         551 :       break;
+     569             :     }
+     570             : 
+     571         517 :     case common::ACCELERATION_HDG_RATE: {
+     572         517 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     573         517 :       break;
+     574             :     }
+     575             : 
+     576        1149 :     case common::ATTITUDE: {
+     577        1149 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE);
+     578        1149 :       break;
+     579             :     }
+     580             : 
+     581       20712 :     case common::ATTITUDE_RATE: {
+     582       20712 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     583       20712 :       break;
+     584             :     }
+     585             : 
+     586        1204 :     case common::CONTROL_GROUP: {
+     587        1204 :       SE3Controller(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     588        1204 :       break;
+     589             :     }
+     590             : 
+     591        1211 :     case common::ACTUATORS_CMD: {
+     592        1211 :       SE3Controller(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     593        1211 :       break;
+     594             :     }
+     595             : 
+     596       28490 :     default: {
+     597             :     }
+     598             :   }
+     599             : 
+     600       28490 :   return last_control_output_;
+     601             : }
+     602             : 
+     603             : //}
+     604             : 
+     605             : /* //{ getStatus() */
+     606             : 
+     607        2868 : const mrs_msgs::ControllerStatus Se3Controller::getStatus() {
+     608             : 
+     609        2868 :   mrs_msgs::ControllerStatus controller_status;
+     610             : 
+     611        2868 :   controller_status.active = is_active_;
+     612             : 
+     613        2868 :   return controller_status;
+     614             : }
+     615             : 
+     616             : //}
+     617             : 
+     618             : /* switchOdometrySource() //{ */
+     619             : 
+     620           0 : void Se3Controller::switchOdometrySource(const mrs_msgs::UavState& new_uav_state) {
+     621             : 
+     622           0 :   ROS_INFO("[Se3Controller]: switching the odometry source");
+     623             : 
+     624           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     625             : 
+     626             :   // | ----- transform world disturabances to the new frame ----- |
+     627             : 
+     628           0 :   geometry_msgs::Vector3Stamped world_integrals;
+     629             : 
+     630           0 :   world_integrals.header.stamp    = ros::Time::now();
+     631           0 :   world_integrals.header.frame_id = uav_state.header.frame_id;
+     632             : 
+     633           0 :   world_integrals.vector.x = Iw_w_(0);
+     634           0 :   world_integrals.vector.y = Iw_w_(1);
+     635           0 :   world_integrals.vector.z = 0;
+     636             : 
+     637           0 :   auto res = common_handlers_->transformer->transformSingle(world_integrals, new_uav_state.header.frame_id);
+     638             : 
+     639           0 :   if (res) {
+     640             : 
+     641           0 :     std::scoped_lock lock(mutex_integrals_);
+     642             : 
+     643           0 :     Iw_w_(0) = res.value().vector.x;
+     644           0 :     Iw_w_(1) = res.value().vector.y;
+     645             : 
+     646             :   } else {
+     647             : 
+     648           0 :     ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: could not transform world integral to the new frame");
+     649             : 
+     650           0 :     std::scoped_lock lock(mutex_integrals_);
+     651             : 
+     652           0 :     Iw_w_(0) = 0;
+     653           0 :     Iw_w_(1) = 0;
+     654             :   }
+     655           0 : }
+     656             : 
+     657             : //}
+     658             : 
+     659             : /* resetDisturbanceEstimators() //{ */
+     660             : 
+     661           0 : void Se3Controller::resetDisturbanceEstimators(void) {
+     662             : 
+     663           0 :   std::scoped_lock lock(mutex_integrals_);
+     664             : 
+     665           0 :   Iw_w_ = Eigen::Vector2d::Zero(2);
+     666           0 :   Ib_b_ = Eigen::Vector2d::Zero(2);
+     667           0 : }
+     668             : 
+     669             : //}
+     670             : 
+     671             : /* setConstraints() //{ */
+     672             : 
+     673         372 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr Se3Controller::setConstraints([
+     674             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+     675             : 
+     676         372 :   if (!is_initialized_) {
+     677           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     678             :   }
+     679             : 
+     680         372 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     681             : 
+     682         372 :   ROS_INFO("[Se3Controller]: updating constraints");
+     683             : 
+     684         744 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     685         372 :   res.success = true;
+     686         372 :   res.message = "constraints updated";
+     687             : 
+     688         372 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     689             : }
+     690             : 
+     691             : //}
+     692             : 
+     693             : // --------------------------------------------------------------
+     694             : // |                         controllers                        |
+     695             : // --------------------------------------------------------------
+     696             : 
+     697             : /* SE3Controller() //{ */
+     698             : 
+     699       25344 : void Se3Controller::SE3Controller(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command, const double& dt,
+     700             :                                   const common::CONTROL_OUTPUT& output_modality) {
+     701             : 
+     702       50688 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     703       25344 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     704       25344 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     705             : 
+     706             :   // | ----------------- get the current heading ---------------- |
+     707             : 
+     708       25344 :   double uav_heading = getHeadingSafely(uav_state, tracker_command);
+     709             : 
+     710             :   // --------------------------------------------------------------
+     711             :   // |          load the control reference and estimates          |
+     712             :   // --------------------------------------------------------------
+     713             : 
+     714             :   // Rp - position reference in global frame
+     715             :   // Rv - velocity reference in global frame
+     716             :   // Ra - velocity reference in global frame
+     717             : 
+     718       25344 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     719       25344 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     720       25344 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     721             : 
+     722       25344 :   if (tracker_command.use_position_vertical || tracker_command.use_position_horizontal) {
+     723             : 
+     724       25344 :     if (tracker_command.use_position_horizontal) {
+     725       25344 :       Rp(0) = tracker_command.position.x;
+     726       25344 :       Rp(1) = tracker_command.position.y;
+     727             :     } else {
+     728           0 :       Rv(0) = 0;
+     729           0 :       Rv(1) = 0;
+     730             :     }
+     731             : 
+     732       25344 :     if (tracker_command.use_position_vertical) {
+     733       25344 :       Rp(2) = tracker_command.position.z;
+     734             :     } else {
+     735           0 :       Rv(2) = 0;
+     736             :     }
+     737             :   }
+     738             : 
+     739       25344 :   if (tracker_command.use_velocity_horizontal) {
+     740       25344 :     Rv(0) = tracker_command.velocity.x;
+     741       25344 :     Rv(1) = tracker_command.velocity.y;
+     742             :   } else {
+     743           0 :     Rv(0) = 0;
+     744           0 :     Rv(1) = 0;
+     745             :   }
+     746             : 
+     747       25344 :   if (tracker_command.use_velocity_vertical) {
+     748       25344 :     Rv(2) = tracker_command.velocity.z;
+     749             :   } else {
+     750           0 :     Rv(2) = 0;
+     751             :   }
+     752             : 
+     753       25344 :   if (tracker_command.use_acceleration) {
+     754       25340 :     Ra << tracker_command.acceleration.x, tracker_command.acceleration.y, tracker_command.acceleration.z;
+     755             :   } else {
+     756           4 :     Ra << 0, 0, 0;
+     757             :   }
+     758             : 
+     759             :   // | ------ store the estimated values from the uav state ----- |
+     760             : 
+     761             :   // Op - position in global frame
+     762             :   // Ov - velocity in global frame
+     763       25344 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     764       25344 :   Eigen::Vector3d Ov(uav_state.velocity.linear.x, uav_state.velocity.linear.y, uav_state.velocity.linear.z);
+     765             : 
+     766             :   // R - current uav attitude
+     767       25344 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     768             : 
+     769             :   // Ow - UAV angular rate
+     770       25344 :   Eigen::Vector3d Ow(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     771             : 
+     772             :   // | -------------- calculate the control errors -------------- |
+     773             : 
+     774             :   // position control error
+     775       25344 :   Eigen::Vector3d Ep(0, 0, 0);
+     776             : 
+     777       25344 :   if (tracker_command.use_position_horizontal || tracker_command.use_position_vertical) {
+     778       25344 :     Ep = Rp - Op;
+     779             :   }
+     780             : 
+     781             :   // velocity control error
+     782       25344 :   Eigen::Vector3d Ev(0, 0, 0);
+     783             : 
+     784       25344 :   if (tracker_command.use_velocity_horizontal || tracker_command.use_velocity_vertical ||
+     785           0 :       tracker_command.use_position_vertical) {  // use_position_vertical = true, not a mistake, this provides dampening
+     786       25344 :     Ev = Rv - Ov;
+     787             :   }
+     788             : 
+     789             :   // | --------------------- load the gains --------------------- |
+     790             : 
+     791       25344 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+     792             : 
+     793       25344 :   Eigen::Vector3d Ka(0, 0, 0);
+     794       25344 :   Eigen::Array3d  Kp(0, 0, 0);
+     795       25344 :   Eigen::Array3d  Kv(0, 0, 0);
+     796       25344 :   Eigen::Array3d  Kq(0, 0, 0);
+     797       25344 :   Eigen::Array3d  Kw(0, 0, 0);
+     798             : 
+     799             :   {
+     800       25344 :     std::scoped_lock lock(mutex_gains_);
+     801             : 
+     802       25344 :     if (tracker_command.use_position_horizontal) {
+     803       25344 :       Kp(0) = gains.kpxy;
+     804       25344 :       Kp(1) = gains.kpxy;
+     805             :     } else {
+     806           0 :       Kp(0) = 0;
+     807           0 :       Kp(1) = 0;
+     808             :     }
+     809             : 
+     810       25344 :     if (tracker_command.use_position_vertical) {
+     811       25344 :       Kp(2) = gains.kpz;
+     812             :     } else {
+     813           0 :       Kp(2) = 0;
+     814             :     }
+     815             : 
+     816       25344 :     if (tracker_command.use_velocity_horizontal) {
+     817       25344 :       Kv(0) = gains.kvxy;
+     818       25344 :       Kv(1) = gains.kvxy;
+     819             :     } else {
+     820           0 :       Kv(0) = 0;
+     821           0 :       Kv(1) = 0;
+     822             :     }
+     823             : 
+     824             :     // special case: if want to control z-pos but not the velocity => at least provide z dampening, therefore kvz_
+     825       25344 :     if (tracker_command.use_velocity_vertical || tracker_command.use_position_vertical) {
+     826       25344 :       Kv(2) = gains.kvz;
+     827             :     } else {
+     828           0 :       Kv(2) = 0;
+     829             :     }
+     830             : 
+     831       25344 :     if (tracker_command.use_acceleration) {
+     832       25340 :       Ka << gains.kaxy, gains.kaxy, gains.kaz;
+     833             :     } else {
+     834           4 :       Ka << 0, 0, 0;
+     835             :     }
+     836             : 
+     837       25344 :     if (!tracker_command.use_attitude_rate) {
+     838       25344 :       Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+     839             :     }
+     840             : 
+     841       25344 :     Kw(0) = gains.kw_roll_pitch;
+     842       25344 :     Kw(1) = gains.kw_roll_pitch;
+     843       25344 :     Kw(2) = gains.kw_yaw;
+     844             :   }
+     845             : 
+     846       25344 :   Kp = Kp * (_uav_mass_ + uav_mass_difference_);
+     847       25344 :   Kv = Kv * (_uav_mass_ + uav_mass_difference_);
+     848             : 
+     849             :   // | --------------- desired orientation matrix --------------- |
+     850             : 
+     851             :   // get body integral in the world frame
+     852             : 
+     853       25344 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+     854             : 
+     855             :   {
+     856       50688 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+     857             : 
+     858       25344 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+     859       25344 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+     860       25344 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+     861       25344 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+     862       25344 :     Ib_b_stamped.vector.z        = 0;
+     863             : 
+     864       50688 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+     865             : 
+     866       25344 :     if (res) {
+     867       25344 :       Ib_w(0) = res.value().vector.x;
+     868       25344 :       Ib_w(1) = res.value().vector.y;
+     869             :     } else {
+     870           0 :       ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: could not transform the Ib_b_ to the world frame");
+     871             :     }
+     872             :   }
+     873             : 
+     874             :   // construct the desired force vector
+     875             : 
+     876       25344 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+     877             : 
+     878       25344 :   Eigen::Vector3d feed_forward      = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+     879       25344 :   Eigen::Vector3d position_feedback = Kp * Ep.array();
+     880       25344 :   Eigen::Vector3d velocity_feedback = Kv * Ev.array();
+     881       25344 :   Eigen::Vector3d integral_feedback;
+     882             :   {
+     883       25344 :     std::scoped_lock lock(mutex_integrals_);
+     884             : 
+     885       25344 :     integral_feedback << Ib_w(0) + Iw_w_(0), Ib_w(1) + Iw_w_(1), 0;
+     886             :   }
+     887             : 
+     888             :   // --------------------------------------------------------------
+     889             :   // |                 integrators and estimators                 |
+     890             :   // --------------------------------------------------------------
+     891             : 
+     892             :   /* world error integrator //{ */
+     893             : 
+     894             :   // --------------------------------------------------------------
+     895             :   // |                  integrate the world error                 |
+     896             :   // --------------------------------------------------------------
+     897             : 
+     898             :   {
+     899       50688 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+     900             : 
+     901       25344 :     Eigen::Vector3d integration_switch(1, 1, 0);
+     902             : 
+     903             :     // integrate the world error
+     904       25344 :     if (tracker_command.use_position_horizontal) {
+     905       25344 :       Iw_w_ += gains.kiwxy * Ep.head(2) * dt;
+     906           0 :     } else if (tracker_command.use_velocity_horizontal) {
+     907           0 :       Iw_w_ += gains.kiwxy * Ev.head(2) * dt;
+     908             :     }
+     909             : 
+     910             :     // saturate the world X
+     911       25344 :     bool world_integral_saturated = false;
+     912       25344 :     if (!std::isfinite(Iw_w_(0))) {
+     913           0 :       Iw_w_(0) = 0;
+     914           0 :       ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: NaN detected in variable 'Iw_w_(0)', setting it to 0!!!");
+     915       25344 :     } else if (Iw_w_(0) > gains.kiwxy_lim) {
+     916           0 :       Iw_w_(0)                 = gains.kiwxy_lim;
+     917           0 :       world_integral_saturated = true;
+     918       25344 :     } else if (Iw_w_(0) < -gains.kiwxy_lim) {
+     919           0 :       Iw_w_(0)                 = -gains.kiwxy_lim;
+     920           0 :       world_integral_saturated = true;
+     921             :     }
+     922             : 
+     923       25344 :     if (gains.kiwxy_lim >= 0 && world_integral_saturated) {
+     924           0 :       ROS_WARN_THROTTLE(1.0, "[Se3Controller]: SE3's world X integral is being saturated!");
+     925             :     }
+     926             : 
+     927             :     // saturate the world Y
+     928       25344 :     world_integral_saturated = false;
+     929       25344 :     if (!std::isfinite(Iw_w_(1))) {
+     930           0 :       Iw_w_(1) = 0;
+     931           0 :       ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: NaN detected in variable 'Iw_w_(1)', setting it to 0!!!");
+     932       25344 :     } else if (Iw_w_(1) > gains.kiwxy_lim) {
+     933           0 :       Iw_w_(1)                 = gains.kiwxy_lim;
+     934           0 :       world_integral_saturated = true;
+     935       25344 :     } else if (Iw_w_(1) < -gains.kiwxy_lim) {
+     936           0 :       Iw_w_(1)                 = -gains.kiwxy_lim;
+     937           0 :       world_integral_saturated = true;
+     938             :     }
+     939             : 
+     940       25344 :     if (gains.kiwxy_lim >= 0 && world_integral_saturated) {
+     941           0 :       ROS_WARN_THROTTLE(1.0, "[Se3Controller]: SE3's world Y integral is being saturated!");
+     942             :     }
+     943             :   }
+     944             : 
+     945             :   //}
+     946             : 
+     947             :   /* body error integrator //{ */
+     948             : 
+     949             :   // --------------------------------------------------------------
+     950             :   // |                  integrate the body error                  |
+     951             :   // --------------------------------------------------------------
+     952             : 
+     953             :   {
+     954       50688 :     std::scoped_lock lock(mutex_gains_);
+     955             : 
+     956       25344 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+     957       25344 :     Eigen::Vector2d Ev_fcu_untilted = Eigen::Vector2d(0, 0);  // velocity error in the untilted frame of the UAV
+     958             : 
+     959             :     // get the position control error in the fcu_untilted frame
+     960             :     {
+     961             : 
+     962       50688 :       geometry_msgs::Vector3Stamped Ep_stamped;
+     963             : 
+     964       25344 :       Ep_stamped.header.stamp    = ros::Time::now();
+     965       25344 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+     966       25344 :       Ep_stamped.vector.x        = Ep(0);
+     967       25344 :       Ep_stamped.vector.y        = Ep(1);
+     968       25344 :       Ep_stamped.vector.z        = Ep(2);
+     969             : 
+     970       76032 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+     971             : 
+     972       25344 :       if (res) {
+     973       25344 :         Ep_fcu_untilted(0) = res.value().vector.x;
+     974       25344 :         Ep_fcu_untilted(1) = res.value().vector.y;
+     975             :       } else {
+     976           0 :         ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: could not transform the position error to fcu_untilted");
+     977             :       }
+     978             :     }
+     979             : 
+     980             :     // get the velocity control error in the fcu_untilted frame
+     981             :     {
+     982       50688 :       geometry_msgs::Vector3Stamped Ev_stamped;
+     983             : 
+     984       25344 :       Ev_stamped.header.stamp    = ros::Time::now();
+     985       25344 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+     986       25344 :       Ev_stamped.vector.x        = Ev(0);
+     987       25344 :       Ev_stamped.vector.y        = Ev(1);
+     988       25344 :       Ev_stamped.vector.z        = Ev(2);
+     989             : 
+     990       76032 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+     991             : 
+     992       25344 :       if (res) {
+     993       25344 :         Ev_fcu_untilted(0) = res.value().vector.x;
+     994       25344 :         Ev_fcu_untilted(1) = res.value().vector.x;
+     995             :       } else {
+     996           0 :         ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: could not transform the velocity error to fcu_untilted");
+     997             :       }
+     998             :     }
+     999             : 
+    1000             :     // integrate the body error
+    1001       25344 :     if (tracker_command.use_position_horizontal) {
+    1002       25344 :       Ib_b_ += gains.kibxy * Ep_fcu_untilted * dt;
+    1003           0 :     } else if (tracker_command.use_velocity_horizontal) {
+    1004           0 :       Ib_b_ += gains.kibxy * Ev_fcu_untilted * dt;
+    1005             :     }
+    1006             : 
+    1007             :     // saturate the body
+    1008       25344 :     bool body_integral_saturated = false;
+    1009       25344 :     if (!std::isfinite(Ib_b_(0))) {
+    1010           0 :       Ib_b_(0) = 0;
+    1011           0 :       ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: NaN detected in variable 'Ib_b_(0)', setting it to 0!!!");
+    1012       25344 :     } else if (Ib_b_(0) > gains.kibxy_lim) {
+    1013           0 :       Ib_b_(0)                = gains.kibxy_lim;
+    1014           0 :       body_integral_saturated = true;
+    1015       25344 :     } else if (Ib_b_(0) < -gains.kibxy_lim) {
+    1016           0 :       Ib_b_(0)                = -gains.kibxy_lim;
+    1017           0 :       body_integral_saturated = true;
+    1018             :     }
+    1019             : 
+    1020       25344 :     if (gains.kibxy_lim > 0 && body_integral_saturated) {
+    1021           0 :       ROS_WARN_THROTTLE(1.0, "[Se3Controller]: SE3's body pitch integral is being saturated!");
+    1022             :     }
+    1023             : 
+    1024             :     // saturate the body
+    1025       25344 :     body_integral_saturated = false;
+    1026       25344 :     if (!std::isfinite(Ib_b_(1))) {
+    1027           0 :       Ib_b_(1) = 0;
+    1028           0 :       ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: NaN detected in variable 'Ib_b_(1)', setting it to 0!!!");
+    1029       25344 :     } else if (Ib_b_(1) > gains.kibxy_lim) {
+    1030           0 :       Ib_b_(1)                = gains.kibxy_lim;
+    1031           0 :       body_integral_saturated = true;
+    1032       25344 :     } else if (Ib_b_(1) < -gains.kibxy_lim) {
+    1033           0 :       Ib_b_(1)                = -gains.kibxy_lim;
+    1034           0 :       body_integral_saturated = true;
+    1035             :     }
+    1036             : 
+    1037       25344 :     if (gains.kibxy_lim > 0 && body_integral_saturated) {
+    1038           0 :       ROS_WARN_THROTTLE(1.0, "[Se3Controller]: SE3's body roll integral is being saturated!");
+    1039             :     }
+    1040             :   }
+    1041             : 
+    1042             :   //}
+    1043             : 
+    1044       25344 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1045             : 
+    1046        1068 :     Eigen::Vector3d des_acc = (position_feedback + velocity_feedback + integral_feedback) / total_mass + Ra;
+    1047             : 
+    1048        1068 :     if (output_modality == common::ACCELERATION_HDG) {
+    1049             : 
+    1050        1102 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1051             : 
+    1052         551 :       cmd.acceleration.x = des_acc(0);
+    1053         551 :       cmd.acceleration.y = des_acc(1);
+    1054         551 :       cmd.acceleration.z = des_acc(2);
+    1055             : 
+    1056         551 :       cmd.heading = tracker_command.heading;
+    1057             : 
+    1058         551 :       last_control_output_.control_output = cmd;
+    1059             : 
+    1060             :     } else {
+    1061             : 
+    1062         517 :       double des_hdg_ff = 0;
+    1063             : 
+    1064         517 :       if (tracker_command.use_heading_rate) {
+    1065         517 :         des_hdg_ff = tracker_command.heading_rate;
+    1066             :       }
+    1067             : 
+    1068        1034 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1069             : 
+    1070         517 :       cmd.acceleration.x = des_acc(0);
+    1071         517 :       cmd.acceleration.y = des_acc(1);
+    1072         517 :       cmd.acceleration.z = des_acc(2);
+    1073             : 
+    1074         517 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1075             : 
+    1076         517 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1077             : 
+    1078         517 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1079             : 
+    1080         517 :       cmd.heading_rate = des_hdg_rate;
+    1081             : 
+    1082         517 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1083             : 
+    1084         517 :       last_control_output_.control_output = cmd;
+    1085             :     }
+    1086             : 
+    1087             :     // | -------------- unbiased desired acceleration ------------- |
+    1088             : 
+    1089        1068 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1090             : 
+    1091             :     {
+    1092             : 
+    1093        1068 :       Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1094             : 
+    1095        2136 :       geometry_msgs::Vector3Stamped world_accel;
+    1096             : 
+    1097        1068 :       world_accel.header.stamp    = ros::Time::now();
+    1098        1068 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1099        1068 :       world_accel.vector.x        = unbiased_des_acc_world(0);
+    1100        1068 :       world_accel.vector.y        = unbiased_des_acc_world(1);
+    1101        1068 :       world_accel.vector.z        = unbiased_des_acc_world(2);
+    1102             : 
+    1103        3204 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1104             : 
+    1105        1068 :       if (res) {
+    1106        1068 :         unbiased_des_acc << res.value().vector.x, res.value().vector.y, res.value().vector.z;
+    1107             :       }
+    1108             :     }
+    1109             : 
+    1110             :     // fill the unbiased desired accelerations
+    1111        1068 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1112             : 
+    1113             :     // | ----------------- fill in the diagnostics ---------------- |
+    1114             : 
+    1115        1068 :     last_control_output_.diagnostics.ramping_up = false;
+    1116             : 
+    1117        1068 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1118        1068 :     last_control_output_.diagnostics.mass_difference = 0;
+    1119        1068 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1120             : 
+    1121        1068 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1122             : 
+    1123        1068 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1124        1068 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1125             : 
+    1126        1068 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1127        1068 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1128             : 
+    1129        1068 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1130        1068 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1131             : 
+    1132        1068 :     last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1133             : 
+    1134        1068 :     last_control_output_.diagnostics.controller = "Se3Controller";
+    1135             : 
+    1136        1068 :     return;
+    1137             :   }
+    1138             : 
+    1139             :   /* mass estimatior //{ */
+    1140             : 
+    1141             :   // --------------------------------------------------------------
+    1142             :   // |                integrate the mass difference               |
+    1143             :   // --------------------------------------------------------------
+    1144             : 
+    1145             :   {
+    1146       48552 :     std::scoped_lock lock(mutex_gains_);
+    1147             : 
+    1148       24276 :     if (tracker_command.use_position_vertical && !rampup_active_) {
+    1149       24260 :       uav_mass_difference_ += gains.km * Ep(2) * dt;
+    1150             :     }
+    1151             : 
+    1152             :     // saturate the mass estimator
+    1153       24276 :     bool uav_mass_saturated = false;
+    1154       24276 :     if (!std::isfinite(uav_mass_difference_)) {
+    1155           0 :       uav_mass_difference_ = 0;
+    1156           0 :       ROS_WARN_THROTTLE(1.0, "[Se3Controller]: NaN detected in variable 'uav_mass_difference_', setting it to 0 and returning!!!");
+    1157       24276 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1158           0 :       uav_mass_difference_ = gains.km_lim;
+    1159           0 :       uav_mass_saturated   = true;
+    1160       24276 :     } else if (uav_mass_difference_ < -gains.km_lim) {
+    1161           0 :       uav_mass_difference_ = -gains.km_lim;
+    1162           0 :       uav_mass_saturated   = true;
+    1163             :     }
+    1164             : 
+    1165       24276 :     if (uav_mass_saturated) {
+    1166           0 :       ROS_WARN_THROTTLE(1.0, "[Se3Controller]: The UAV mass difference is being saturated to %.2f!", uav_mass_difference_);
+    1167             :     }
+    1168             :   }
+    1169             : 
+    1170             :   //}
+    1171             : 
+    1172       24276 :   Eigen::Vector3d f = position_feedback + velocity_feedback + integral_feedback + feed_forward;
+    1173             : 
+    1174             :   // | ----------- limiting the downwards acceleration ---------- |
+    1175             :   // the downwards force produced by the position and the acceleration feedback should not be larger than the gravity
+    1176             : 
+    1177             :   // if the downwards part of the force is close to counter-act the gravity acceleration
+    1178       24276 :   if (f(2) < 0) {
+    1179             : 
+    1180           0 :     ROS_WARN_THROTTLE(1.0, "[Se3Controller]: the calculated downwards desired force is negative (%.2f) -> mitigating flip", f(2));
+    1181             : 
+    1182           0 :     f << 0, 0, 1;
+    1183             :   }
+    1184             : 
+    1185             :   // | ------------------- sanitize tilt angle ------------------ |
+    1186             : 
+    1187       24276 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1188             : 
+    1189       24276 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "Se3Controller");
+    1190             : 
+    1191       24276 :   if (!f_normed_sanitized) {
+    1192             : 
+    1193           0 :     ROS_INFO("[Se3Controller]: position feedback: [%.2f, %.2f, %.2f]", position_feedback(0), position_feedback(1), position_feedback(2));
+    1194           0 :     ROS_INFO("[Se3Controller]: velocity feedback: [%.2f, %.2f, %.2f]", velocity_feedback(0), velocity_feedback(1), velocity_feedback(2));
+    1195           0 :     ROS_INFO("[Se3Controller]: integral feedback: [%.2f, %.2f, %.2f]", integral_feedback(0), integral_feedback(1), integral_feedback(2));
+    1196           0 :     ROS_INFO("[Se3Controller]: tracker_cmd: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", tracker_command.position.x, tracker_command.position.y,
+    1197             :              tracker_command.position.z, tracker_command.heading);
+    1198           0 :     ROS_INFO("[Se3Controller]: odometry: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+    1199             :              uav_state.pose.position.z, uav_heading);
+    1200             : 
+    1201           0 :     return;
+    1202             :   }
+    1203             : 
+    1204       24276 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1205             : 
+    1206             :   // --------------------------------------------------------------
+    1207             :   // |               desired orientation + throttle               |
+    1208             :   // --------------------------------------------------------------
+    1209             : 
+    1210             :   // | ------------------- desired orientation ------------------ |
+    1211             : 
+    1212       24276 :   Eigen::Matrix3d Rd;
+    1213             : 
+    1214       24276 :   if (tracker_command.use_orientation) {
+    1215             : 
+    1216             :     // fill in the desired orientation based on the desired orientation from the control command
+    1217           0 :     Rd = mrs_lib::AttitudeConverter(tracker_command.orientation);
+    1218             : 
+    1219           0 :     if (tracker_command.use_heading) {
+    1220             :       try {
+    1221           0 :         Rd = mrs_lib::AttitudeConverter(Rd).setHeading(tracker_command.heading);
+    1222             :       }
+    1223           0 :       catch (...) {
+    1224           0 :         ROS_ERROR_THROTTLE(1.0, "[Se3Controller]: could not set the desired heading");
+    1225             :       }
+    1226             :     }
+    1227             : 
+    1228             :   } else {
+    1229             : 
+    1230       24276 :     Eigen::Vector3d bxd;  // desired heading vector
+    1231             : 
+    1232       24276 :     if (tracker_command.use_heading) {
+    1233       24276 :       bxd << cos(tracker_command.heading), sin(tracker_command.heading), 0;
+    1234             :     } else {
+    1235           0 :       ROS_WARN_THROTTLE(10.0, "[Se3Controller]: desired heading was not specified, using current heading instead!");
+    1236           0 :       bxd << cos(uav_heading), sin(uav_heading), 0;
+    1237             :     }
+    1238             : 
+    1239       24276 :     Rd = common::so3transform(f_normed, bxd, drs_params.rotation_type == 1);
+    1240             :   }
+    1241             : 
+    1242             :   // | -------------------- desired throttle -------------------- |
+    1243             : 
+    1244       24276 :   double desired_thrust_force = f.dot(R.col(2));
+    1245       24276 :   double throttle             = 0;
+    1246             : 
+    1247       24276 :   if (tracker_command.use_throttle) {
+    1248             : 
+    1249             :     // the throttle is overriden from the tracker command
+    1250           0 :     throttle = tracker_command.throttle;
+    1251             : 
+    1252       24276 :   } else if (rampup_active_) {
+    1253             : 
+    1254             :     // deactivate the rampup when the times up
+    1255          16 :     if (std::abs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1256             : 
+    1257          14 :       rampup_active_ = false;
+    1258             : 
+    1259          14 :       ROS_INFO("[Se3Controller]: rampup finished");
+    1260             : 
+    1261             :     } else {
+    1262             : 
+    1263           2 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1264             : 
+    1265           2 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1266             : 
+    1267           2 :       rampup_last_time_ = ros::Time::now();
+    1268             : 
+    1269           2 :       throttle = rampup_throttle_;
+    1270             : 
+    1271           2 :       ROS_INFO_THROTTLE(0.1, "[Se3Controller]: ramping up throttle, %.4f", throttle);
+    1272             :     }
+    1273             : 
+    1274             :   } else {
+    1275             : 
+    1276       24260 :     if (desired_thrust_force >= 0) {
+    1277       24260 :       throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, desired_thrust_force);
+    1278             :     } else {
+    1279           0 :       ROS_WARN_THROTTLE(1.0, "[Se3Controller]: just so you know, the desired throttle force is negative (%.2f)", desired_thrust_force);
+    1280             :     }
+    1281             :   }
+    1282             : 
+    1283             :   // | ------------------- throttle saturation ------------------ |
+    1284             : 
+    1285       24276 :   bool throttle_saturated = false;
+    1286             : 
+    1287       24276 :   if (!std::isfinite(throttle)) {
+    1288             : 
+    1289           0 :     ROS_ERROR("[Se3Controller]: NaN detected in variable 'throttle'!!!");
+    1290           0 :     return;
+    1291             : 
+    1292       24276 :   } else if (throttle > _throttle_saturation_) {
+    1293           0 :     throttle = _throttle_saturation_;
+    1294           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: saturating throttle to %.2f", _throttle_saturation_);
+    1295       24276 :   } else if (throttle < 0.0) {
+    1296           0 :     throttle = 0.0;
+    1297           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: saturating throttle to 0.0");
+    1298             :   }
+    1299             : 
+    1300       24276 :   if (throttle_saturated) {
+    1301           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: ---------------------------");
+    1302           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: desired state: pos [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", tracker_command.position.x, tracker_command.position.y,
+    1303             :                       tracker_command.position.z, tracker_command.heading);
+    1304           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: desired state: vel [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", tracker_command.velocity.x, tracker_command.velocity.y,
+    1305             :                       tracker_command.velocity.z, tracker_command.heading_rate);
+    1306           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: desired state: acc [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", tracker_command.acceleration.x,
+    1307             :                       tracker_command.acceleration.y, tracker_command.acceleration.z, tracker_command.heading_acceleration);
+    1308           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: desired state: jerk [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", tracker_command.jerk.x, tracker_command.jerk.y,
+    1309             :                       tracker_command.jerk.z, tracker_command.heading_jerk);
+    1310           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: ---------------------------");
+    1311           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: current state: pos [x: %.2f, y: %.2f, z: %.2f, hdg: %.2f]", uav_state.pose.position.x, uav_state.pose.position.y,
+    1312             :                       uav_state.pose.position.z, uav_heading);
+    1313           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: current state: vel [x: %.2f, y: %.2f, z: %.2f, yaw rate: %.2f]", uav_state.velocity.linear.x,
+    1314             :                       uav_state.velocity.linear.y, uav_state.velocity.linear.z, uav_state.velocity.angular.z);
+    1315           0 :     ROS_WARN_THROTTLE(0.1, "[Se3Controller]: ---------------------------");
+    1316             :   }
+    1317             : 
+    1318             :   // | -------------- unbiased desired acceleration ------------- |
+    1319             : 
+    1320       24276 :   Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1321             : 
+    1322             :   {
+    1323       24276 :     Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1324             : 
+    1325       48552 :     geometry_msgs::Vector3Stamped world_accel;
+    1326             : 
+    1327       24276 :     world_accel.header.stamp    = ros::Time::now();
+    1328       24276 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1329       24276 :     world_accel.vector.x        = unbiased_des_acc_world(0);
+    1330       24276 :     world_accel.vector.y        = unbiased_des_acc_world(1);
+    1331       24276 :     world_accel.vector.z        = unbiased_des_acc_world(2);
+    1332             : 
+    1333       72828 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1334             : 
+    1335       24276 :     if (res) {
+    1336       24276 :       unbiased_des_acc << res.value().vector.x, res.value().vector.y, res.value().vector.z;
+    1337             :     }
+    1338             :   }
+    1339             : 
+    1340             :   // | --------------- fill the resulting command --------------- |
+    1341             : 
+    1342             :   // fill the desired orientation for the tilt error check
+    1343       24276 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1344             : 
+    1345             :   // fill the unbiased desired accelerations
+    1346       24276 :   last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1347             : 
+    1348             :   // | ----------------- fill in the diagnostics ---------------- |
+    1349             : 
+    1350       24276 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1351             : 
+    1352       24276 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1353       24276 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1354       24276 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1355             : 
+    1356       24276 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1357             : 
+    1358       24276 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1359       24276 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1360             : 
+    1361       24276 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1362       24276 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1363             : 
+    1364       24276 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1365       24276 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1366             : 
+    1367       24276 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1368             : 
+    1369       24276 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1370             : 
+    1371             :   // | ------------ construct the attitude reference ------------ |
+    1372             : 
+    1373       24276 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1374             : 
+    1375       24276 :   attitude_cmd.stamp       = ros::Time::now();
+    1376       24276 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1377       24276 :   attitude_cmd.throttle    = throttle;
+    1378             : 
+    1379       24276 :   if (output_modality == common::ATTITUDE) {
+    1380             : 
+    1381        1149 :     last_control_output_.control_output = attitude_cmd;
+    1382             : 
+    1383        1149 :     return;
+    1384             :   }
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                      attitude control                      |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390       23127 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1391             : 
+    1392       23127 :   if (tracker_command.use_attitude_rate) {
+    1393             : 
+    1394           0 :     rate_feedforward << tracker_command.attitude_rate.x, tracker_command.attitude_rate.y, tracker_command.attitude_rate.z;
+    1395             : 
+    1396       23127 :   } else if (tracker_command.use_heading_rate) {
+    1397             : 
+    1398             :     // to fill in the feed forward yaw rate
+    1399       23123 :     double desired_yaw_rate = 0;
+    1400             : 
+    1401             :     try {
+    1402       23123 :       desired_yaw_rate = mrs_lib::AttitudeConverter(Rd).getYawRateIntrinsic(tracker_command.heading_rate);
+    1403             :     }
+    1404           0 :     catch (...) {
+    1405           0 :       ROS_ERROR("[Se3Controller]: exception caught while calculating the desired_yaw_rate feedforward");
+    1406             :     }
+    1407             : 
+    1408       23123 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1409             :   }
+    1410             : 
+    1411             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1412             : 
+    1413       23127 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1414             : 
+    1415       23127 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1416             : 
+    1417       23099 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: using jerk feedforward");
+    1418             : 
+    1419       23099 :     Eigen::Matrix3d I;
+    1420       23099 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1421       23099 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1422       23099 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1423             :   }
+    1424             : 
+    1425             :   // | --------------- run the attitude controller -------------- |
+    1426             : 
+    1427       23127 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1428             : 
+    1429       23127 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq,
+    1430       46254 :                                                           drs_params.pitch_roll_heading_rate_compensation);
+    1431             : 
+    1432       23127 :   if (!attitude_rate_command) {
+    1433           0 :     return;
+    1434             :   }
+    1435             : 
+    1436             :   // | --------- fill in the already known attitude rate -------- |
+    1437             : 
+    1438             :   {
+    1439             :     try {
+    1440       23127 :       last_control_output_.desired_heading_rate = mrs_lib::AttitudeConverter(R).getHeadingRate(attitude_rate_command->body_rate);
+    1441             :     }
+    1442           0 :     catch (...) {
+    1443             :     }
+    1444             :   }
+    1445             : 
+    1446             :   // | ---------- construct the attitude rate reference --------- |
+    1447             : 
+    1448       23127 :   if (output_modality == common::ATTITUDE_RATE) {
+    1449             : 
+    1450       20712 :     last_control_output_.control_output = attitude_rate_command;
+    1451             : 
+    1452       20712 :     return;
+    1453             :   }
+    1454             : 
+    1455             :   // --------------------------------------------------------------
+    1456             :   // |                    Attitude rate control                   |
+    1457             :   // --------------------------------------------------------------
+    1458             : 
+    1459        2415 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1460             : 
+    1461        2415 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1462             : 
+    1463        2415 :   if (!control_group_command) {
+    1464           0 :     return;
+    1465             :   }
+    1466             : 
+    1467        2415 :   if (output_modality == common::CONTROL_GROUP) {
+    1468             : 
+    1469        1204 :     last_control_output_.control_output = control_group_command;
+    1470             : 
+    1471        1204 :     return;
+    1472             :   }
+    1473             : 
+    1474             :   // --------------------------------------------------------------
+    1475             :   // |                        output mixer                        |
+    1476             :   // --------------------------------------------------------------
+    1477             : 
+    1478        2422 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1479             : 
+    1480        1211 :   last_control_output_.control_output = actuator_cmd;
+    1481             : 
+    1482        1211 :   return;
+    1483             : }
+    1484             : 
+    1485             : //}
+    1486             : 
+    1487             : /* positionPassthrough() //{ */
+    1488             : 
+    1489         226 : void Se3Controller::positionPassthrough(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1490             : 
+    1491         226 :   if (!tracker_command.use_position_vertical || !tracker_command.use_position_horizontal || !tracker_command.use_heading) {
+    1492           0 :     ROS_ERROR("[Se3Controller]: the tracker did not provide position+hdg reference");
+    1493           0 :     return;
+    1494             :   }
+    1495             : 
+    1496         452 :   mrs_msgs::HwApiPositionCmd cmd;
+    1497             : 
+    1498         226 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1499         226 :   cmd.header.stamp    = ros::Time::now();
+    1500             : 
+    1501         226 :   cmd.position = tracker_command.position;
+    1502         226 :   cmd.heading  = tracker_command.heading;
+    1503             : 
+    1504         226 :   last_control_output_.control_output = cmd;
+    1505             : 
+    1506             :   // fill the unbiased desired accelerations
+    1507         226 :   last_control_output_.desired_unbiased_acceleration = {};
+    1508         226 :   last_control_output_.desired_orientation           = {};
+    1509         226 :   last_control_output_.desired_heading_rate          = {};
+    1510             : 
+    1511             :   // | ----------------- fill in the diagnostics ---------------- |
+    1512             : 
+    1513         226 :   last_control_output_.diagnostics.ramping_up = false;
+    1514             : 
+    1515         226 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1516         226 :   last_control_output_.diagnostics.mass_difference = 0;
+    1517             : 
+    1518         226 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1519             : 
+    1520         226 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1521         226 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1522             : 
+    1523         226 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1524         226 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1525             : 
+    1526         226 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1527         226 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1528             : 
+    1529         226 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1530             : 
+    1531         226 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1532             : }
+    1533             : 
+    1534             : //}
+    1535             : 
+    1536             : /* PIDVelocityOutput() //{ */
+    1537             : 
+    1538        2920 : void Se3Controller::PIDVelocityOutput(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command,
+    1539             :                                       const common::CONTROL_OUTPUT& control_output, const double& dt) {
+    1540             : 
+    1541        2920 :   if (!tracker_command.use_position_vertical || !tracker_command.use_position_horizontal || !tracker_command.use_heading) {
+    1542           0 :     ROS_ERROR("[Se3Controller]: the tracker did not provide position+hdg reference");
+    1543           0 :     return;
+    1544             :   }
+    1545             : 
+    1546        2920 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1547        2920 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1548             : 
+    1549        2920 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1550        2920 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1551             : 
+    1552        2920 :   double hdg_ref = tracker_command.heading;
+    1553        2920 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1554             : 
+    1555             :   // | ------------------ velocity feedforward ------------------ |
+    1556             : 
+    1557        2920 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1558             : 
+    1559        2920 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1560        2920 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1561             :   }
+    1562             : 
+    1563             :   // | -------------------------- gains ------------------------- |
+    1564             : 
+    1565        2920 :   Eigen::Vector3d Kp;
+    1566             : 
+    1567             :   {
+    1568        2920 :     std::scoped_lock lock(mutex_gains_);
+    1569             : 
+    1570        2920 :     Kp << gains.kpxy, gains.kpxy, gains.kpz;
+    1571             :   }
+    1572             : 
+    1573             :   // | --------------------- control errors --------------------- |
+    1574             : 
+    1575        2920 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1576             : 
+    1577             :   // | --------------------------- pid -------------------------- |
+    1578             : 
+    1579        2920 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1580        2920 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1581        2920 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1582             : 
+    1583        2920 :   double des_vel_x = position_pid_x_.update(Ep(0), dt);
+    1584        2920 :   double des_vel_y = position_pid_y_.update(Ep(1), dt);
+    1585        2920 :   double des_vel_z = position_pid_z_.update(Ep(2), dt);
+    1586             : 
+    1587             :   // | -------------------- position feedback ------------------- |
+    1588             : 
+    1589        2920 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1590             : 
+    1591        2920 :   if (control_output == common::VELOCITY_HDG) {
+    1592             : 
+    1593             :     // | --------------------- fill the output -------------------- |
+    1594             : 
+    1595        2156 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1596             : 
+    1597        1078 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1598        1078 :     cmd.header.stamp    = ros::Time::now();
+    1599             : 
+    1600        1078 :     cmd.velocity.x = des_vel(0);
+    1601        1078 :     cmd.velocity.y = des_vel(1);
+    1602        1078 :     cmd.velocity.z = des_vel(2);
+    1603             : 
+    1604        1078 :     cmd.heading = tracker_command.heading;
+    1605             : 
+    1606        1078 :     last_control_output_.control_output = cmd;
+    1607             : 
+    1608        1842 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1609             : 
+    1610        1842 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1611             : 
+    1612        1842 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1613             : 
+    1614        1842 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1615             : 
+    1616             :     // | --------------------------- ff --------------------------- |
+    1617             : 
+    1618        1842 :     double des_hdg_ff = 0;
+    1619             : 
+    1620        1842 :     if (tracker_command.use_heading_rate) {
+    1621        1842 :       des_hdg_ff = tracker_command.heading_rate;
+    1622             :     }
+    1623             : 
+    1624             :     // | --------------------- fill the output -------------------- |
+    1625             : 
+    1626        3684 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1627             : 
+    1628        1842 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1629        1842 :     cmd.header.stamp    = ros::Time::now();
+    1630             : 
+    1631        1842 :     cmd.velocity.x = des_vel(0);
+    1632        1842 :     cmd.velocity.y = des_vel(1);
+    1633        1842 :     cmd.velocity.z = des_vel(2);
+    1634             : 
+    1635        1842 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1636             : 
+    1637        1842 :     last_control_output_.control_output = cmd;
+    1638             :   } else {
+    1639             : 
+    1640           0 :     ROS_ERROR("[Se3Controller]: the required output of the position PID is not supported");
+    1641           0 :     return;
+    1642             :   }
+    1643             : 
+    1644             :   // fill the unbiased desired accelerations
+    1645        2920 :   last_control_output_.desired_unbiased_acceleration = {};
+    1646        2920 :   last_control_output_.desired_orientation           = {};
+    1647        2920 :   last_control_output_.desired_heading_rate          = {};
+    1648             : 
+    1649             :   // | ----------------- fill in the diagnostics ---------------- |
+    1650             : 
+    1651        2920 :   last_control_output_.diagnostics.ramping_up = false;
+    1652             : 
+    1653        2920 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1654        2920 :   last_control_output_.diagnostics.mass_difference = 0;
+    1655             : 
+    1656        2920 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1657             : 
+    1658        2920 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1659        2920 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1660             : 
+    1661        2920 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1662        2920 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1663             : 
+    1664        2920 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1665        2920 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1666             : 
+    1667        2920 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1668             : 
+    1669        2920 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1670             : }
+    1671             : 
+    1672             : //}
+    1673             : 
+    1674             : // --------------------------------------------------------------
+    1675             : // |                          callbacks                         |
+    1676             : 
+    1677             : 
+    1678             : /* //{ callbackDrs() */
+    1679             : 
+    1680         189 : void Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig& config, [[maybe_unused]] uint32_t level) {
+    1681             : 
+    1682         189 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1683             : 
+    1684         189 :   ROS_INFO("[Se3Controller]: DRS updated gains");
+    1685         189 : }
+    1686             : 
+    1687             : //}
+    1688             : 
+    1689             : // --------------------------------------------------------------
+    1690             : // |                           timers                           |
+    1691             : // --------------------------------------------------------------
+    1692             : 
+    1693             : /* timerGains() //{ */
+    1694             : 
+    1695        4566 : void Se3Controller::timerGains(const ros::TimerEvent& event) {
+    1696             : 
+    1697        9132 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1698        9132 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::timerGains", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1699             : 
+    1700        4566 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1701        4566 :   auto gains      = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1702             : 
+    1703             :   // When muting the gains, we want to bypass the filter,
+    1704             :   // so it happens immediately.
+    1705        4566 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    1706        4566 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    1707             : 
+    1708        4566 :   mute_gains_ = false;
+    1709             : 
+    1710        4566 :   double dt = (event.current_real - event.last_real).toSec();
+    1711             : 
+    1712        4566 :   if (!std::isfinite(dt) || (dt <= 0) || (dt > 5 * (1.0 / _gain_filtering_rate_))) {
+    1713          27 :     return;
+    1714             :   }
+    1715             : 
+    1716        4539 :   bool updated = false;
+    1717             : 
+    1718        4539 :   gains.kpxy          = calculateGainChange(dt, gains.kpxy, drs_params.kpxy * gain_coeff, bypass_filter, "kpxy", updated);
+    1719        4539 :   gains.kvxy          = calculateGainChange(dt, gains.kvxy, drs_params.kvxy * gain_coeff, bypass_filter, "kvxy", updated);
+    1720        4539 :   gains.kaxy          = calculateGainChange(dt, gains.kaxy, drs_params.kaxy * gain_coeff, bypass_filter, "kaxy", updated);
+    1721        4539 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    1722        4539 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    1723        4539 :   gains.kpz           = calculateGainChange(dt, gains.kpz, drs_params.kpz * gain_coeff, bypass_filter, "kpz", updated);
+    1724        4539 :   gains.kvz           = calculateGainChange(dt, gains.kvz, drs_params.kvz * gain_coeff, bypass_filter, "kvz", updated);
+    1725        4539 :   gains.kaz           = calculateGainChange(dt, gains.kaz, drs_params.kaz * gain_coeff, bypass_filter, "kaz", updated);
+    1726        4539 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    1727        4539 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    1728        4539 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    1729             : 
+    1730             :   // do not apply muting on these gains
+    1731        4539 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    1732        4539 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    1733        4539 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    1734             : 
+    1735        4539 :   mrs_lib::set_mutexed(mutex_gains_, gains, gains_);
+    1736             : 
+    1737             :   // set the gains back to dynamic reconfigure
+    1738             :   // and only do it when some filtering occurs
+    1739        4539 :   if (updated) {
+    1740             : 
+    1741         540 :     drs_params.kpxy          = gains.kpxy;
+    1742         540 :     drs_params.kvxy          = gains.kvxy;
+    1743         540 :     drs_params.kaxy          = gains.kaxy;
+    1744         540 :     drs_params.kiwxy         = gains.kiwxy;
+    1745         540 :     drs_params.kibxy         = gains.kibxy;
+    1746         540 :     drs_params.kpz           = gains.kpz;
+    1747         540 :     drs_params.kvz           = gains.kvz;
+    1748         540 :     drs_params.kaz           = gains.kaz;
+    1749         540 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    1750         540 :     drs_params.kq_yaw        = gains.kq_yaw;
+    1751         540 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    1752         540 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    1753         540 :     drs_params.km            = gains.km;
+    1754         540 :     drs_params.km_lim        = gains.km_lim;
+    1755             : 
+    1756         540 :     drs_->updateConfig(drs_params);
+    1757             : 
+    1758         540 :     ROS_INFO_THROTTLE(10.0, "[Se3Controller]: gains have been updated");
+    1759             :   }
+    1760             : }
+    1761             : 
+    1762             : //}
+    1763             : 
+    1764             : // --------------------------------------------------------------
+    1765             : // |                       other routines                       |
+    1766             : // --------------------------------------------------------------
+    1767             : 
+    1768             : /* calculateGainChange() //{ */
+    1769             : 
+    1770       63546 : double Se3Controller::calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name,
+    1771             :                                           bool& updated) {
+    1772             : 
+    1773       63546 :   double change = desired_value - current_value;
+    1774             : 
+    1775       63546 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    1776       63546 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    1777             : 
+    1778       63546 :   if (!bypass_rate) {
+    1779             : 
+    1780             :     // if current value is near 0...
+    1781             :     double change_in_perc;
+    1782             :     double saturated_change;
+    1783             : 
+    1784       63546 :     if (std::abs(current_value) < 1e-6) {
+    1785           0 :       change *= gains_filter_max_change;
+    1786             :     } else {
+    1787             : 
+    1788       63546 :       saturated_change = change;
+    1789             : 
+    1790       63546 :       change_in_perc = ((current_value + saturated_change) / current_value) - 1.0;
+    1791             : 
+    1792       63546 :       if (change_in_perc > gains_filter_max_change) {
+    1793        1269 :         saturated_change = current_value * gains_filter_max_change;
+    1794       62277 :       } else if (change_in_perc < -gains_filter_max_change) {
+    1795         162 :         saturated_change = current_value * -gains_filter_max_change;
+    1796             :       }
+    1797             : 
+    1798       63546 :       if (std::abs(saturated_change) < std::abs(change) * gains_filter_min_change) {
+    1799          35 :         change *= gains_filter_min_change;
+    1800             :       } else {
+    1801       63511 :         change = saturated_change;
+    1802             :       }
+    1803             :     }
+    1804             :   }
+    1805             : 
+    1806       63546 :   if (std::abs(change) > 1e-3) {
+    1807        1593 :     ROS_DEBUG("[Se3Controller]: changing gain '%s' from %.2f to %.2f", name.c_str(), current_value, desired_value);
+    1808        1593 :     updated = true;
+    1809             :   }
+    1810             : 
+    1811       63546 :   return current_value + change;
+    1812             : }
+    1813             : 
+    1814             : //}
+    1815             : 
+    1816             : /* getHeadingSafely() //{ */
+    1817             : 
+    1818       28264 : double Se3Controller::getHeadingSafely(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1819             : 
+    1820             :   try {
+    1821       28264 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1822             :   }
+    1823           0 :   catch (...) {
+    1824             :   }
+    1825             : 
+    1826             :   try {
+    1827           0 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYaw();
+    1828             :   }
+    1829           0 :   catch (...) {
+    1830             :   }
+    1831             : 
+    1832           0 :   if (tracker_command.use_heading) {
+    1833           0 :     return tracker_command.heading;
+    1834             :   }
+    1835             : 
+    1836           0 :   return 0;
+    1837             : }
+    1838             : 
+    1839             : //}
+    1840             : 
+    1841             : }  // namespace se3_controller
+    1842             : 
+    1843             : }  // namespace mrs_uav_controllers
+    1844             : 
+    1845             : #include <pluginlib/class_list_macros.h>
+    1846          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_controllers::se3_controller::Se3Controller, mrs_uav_managers::Controller)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html b/mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html new file mode 100644 index 0000000000..3721a40cc3 --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html @@ -0,0 +1,482 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/se3_controller.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.gcov.png b/mrs_uav_controllers/src/se3_controller.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b28cdc9e93d4ee501a0f3008a3010cdc4632d341 GIT binary patch literal 5859 zcmV<9798n`P)vX+d=hvT0 zukyloAidU5CcrAFwdz?5*r(nLCg$3OC7FTr%-dc453j$PQ)K_hyT0vN`aC~g# zpvQRb=N!g;u=+A-0gIq5fO8+ZuBh$;&hBhPRTlvVR6QMXHub~oX{iJ1t8_!*c$^MC z?f>=#bfD8UtUwrW4uG@&K1WIbef5kSUDK+7FlLYhFklp_o*~s-y{7;jsF%{|iUBVX zaFU1)#1KHHZ_>0k`>F+wV~irZ8H^L#fms_xz_~h3t!2Q}JOyDsM7syZFh$LC;^?U< zxIE2b$WUKeZx}5Dp)M6uBZKh7id)dI#Pkv(q#u!|Bn39AF|G+eg_*#F5iINx;~Bvu zLP!a3K+b5cC)(#Q&J-iVQ$s`bZA#2&YdVK9G8PHI5t$5DV0nLItP3YG% z?$d^;k4QiTaDV7FVI*RvSpRfNm4;oAe=(!a>{yuTk0rnhtpa_CznIDAdgHHAdITM! zjr&-7Ch%`N!_g!q%*;(oToED0I0XqhX_M0GxQbELlh9gw)dPA4mY^+3fXwgOdFhFn zV!e}7Uc}`UH+dr=#n!5^!7rS@uvWJs_GKpB~A1| z&28r?5$IMl5F;5g#1c3$W@gYvR%sx^$~8a$#J@N#mWXOUtUxo)46fA6QL#woh6OL_ z2=X*(2Xmo~;%_s+^IEfKcx?O}gJ!DJB3$%iUz;2&m8v6eisEpEmTrl}q|;R8QHv4DRV5fVYpPZk)YslWds_L> zxuN<4{%dW7z5PZ&5w@?t{=hiGXJ0HSD3w+wpeUvG%w~+1Hijyj>BQxBTW`38&)e%y zDwhQp`l|$}F3W9u)v;6yt-1gh8nMKtR{*yN$Tq}U^@`cj0b`L4BgQa?&(~<{Oi@?3 zw#?z6jgmmDob+g^rg&0MSupZVTPfiE{=<~V{`c|oeEo6d?f_r!pZ9XP+-nd4P+#CO zJcq6WD#4j9EEWTxp|9QW3xjZFdTKLD_*IB(8>j}&7y)Qt&%3Jbui3ws7@b4sF9^j& zOj!9ahEp^31KUXSwvTM1pXNWkjdgYOGbYkq--D~nq5o|WP>+$UR9mh_zh10Vr+8-S z#p~Vv@|`-xzcsk}16rt7C49V7RWK0~qHZOcPF9`&X0$rv)+Q7?`v`TWJJo-ojWn7v z5oNQbXGGV;4H|vS)O^@@yRqk9#2NP^%sp?k&nGj6IQDG7=M$bT+~9<(TzF=exaYyK zou3c?&wH1+s7cjUCFsyt3%;qtcJ7&D#H$QIlt-_D%;k$@XwQ3t>cRn72m z9H57W{Vf3^Vlw_9)pdaqbQ*Ah#4wWH)W*H+fH}IQ#q}A9Sb6_!z97{w?i&9_f8=Xe zUw>y*#=XdJ(*_GaUSwL}ehL1>=hG z34nTxN)rHTxbkdzOr(Lzo3y6q7(HF9=l1F%s_xZgQ3|BKudDHyW4h_b#{(mzy2L@p zEDNBvjqk3y-jB$X+HPPC0s?Ue#W4vuuS{*8C?GsoJQDqCBe8k0W zNFXO^j;PuV_m;7*!{jMZVj>1&1cI3%H(>_gM>Ixrl#a74C3mz@%@ z>LazaQJTX}V!Yi-wyRpq)Kw46Ozn0y7niQL;-_PsI~Qz9cv8PMU_>er=R!7PEXw!a z5el$;^uQ>I1AWnwk1Og79%Jg?qJC|vdd-9fR1#$ABP1NPZU5@Ur7GZ}1#~T~0fHbE z0@MF){X&G!}v%UFfq6=r(6z8O2fz9ii2ZBN+}|JYtvb*#o97{0V6rM~eP$ci|LwkH>%g31D3^G>{7>JYzq_Mjlr8xFSF_uDLEG z?w)`#ij@DZ(1kx~IQ~5E3GkVqScccC4$t+f-30K%Rn3Y5h2ySqAJK$|0!2|x3jdf z)e(LG(s|M;&1O_KVJsxiv}NP;@saAgaOjZ^eSGK0%uMA$pA5EZj_3hRs03)h=+H1t zeF9TNkI@^s-Y?;qI?eIaG`o>|XPcGVNUePQIT8&#@**I%(Lf6tt9D%P3n*HNMxo#& z0+okJa38u<4n4+uY6sIH%@`k5*N_EJS@nKSz5NKY;+T=#56`&i=J@|7FucCUM-8ZT zktpE|#0)=UsKc5hEJ#0eXg17dWtp&H#FMPhReHcb)z=-P_HtwtYNs$=x%d-()#@k! zb;Z*Xy7#yCv9I}uGYWg)CIxK67!gaGs%jf05MZQG0JOBR5Ic$j_2*0gu5hQiUBba` z@Xgp$-MmWO%X(!xa3fFajoku;CydnB>Jk$#*!W}GwV6VdEMOXF9&n-iXCstd!eOq$ z5E8s0#}`8-cTUmj$p^512T)tU&K=Y<8N&wvYWteq zXp!6m6DBj1Ko@ZeSQXQi z-q3e$7aMCDHZWYWkwg+<9DUTbgZn8dV$;Dd0|GV{3r23uK$4RQc(qrUf zIt@KuU>+~s3tHg*rND4@z5d6=j59wDs#kbWy}$rH)r->n7?YTm_ccQ{q}q)>AQ*5U z4N9JaN_~Mn4S?kdV8EuTLp5`3zR7Jh04mFXgCZ~K=$9$lt7asaJyEEVvd3t$L-ig` zu3M_c7&q}0#;j_5a)tny&ULI*Z3L7z4d|&}D<>=M%(d!|s_fZ?442{#2o<1Z<;ud^ zC>`f`ey4l(0)s&=bMIswG0x>#DIIazWSn^<8Rpgb_Aubl1TN+E1fZ~h=vM{KloCdv z8Kcw393kK-F2jRdB${|(ik*iwWP+Uc*_c-EJWZb13FPj8gAG?|2{cbKxls)HcoE0}$Lj!crnLqB4 z?Ly72!MMa#lQiYE@OX3#KKA5-1Le|M%&fKnW2hk}=F*l>AE~&LpRbxTe59%=UVYU= zZP$Hm6VEs#ZG3=-hZp2$I4f(u5oc#g(baKD0kMsNqH*CWyuWBjB~HsC3B1}?{+ zsm`2_R0L@Rt7fQU2|2%;1yEUaJV_Va?UK$DL|p7GW()87IOX|3hmn*#A0(+-Jl+TZ z28Tv<3w<|01JyOxhtcz)bA22bStgzVRJlHmJr(?_iCiDYJ|xxLJV3(x53c1qW-5RN zs=s6AQ^$-q#EX9YGlsZ($l23wh`R^Mp0V~!5sVt*g)#Hz?3v<5ZPaVh=DlWnMm#`F zU-ejfX4{DWg&`)HG4bIMH}3BJ^KQWf&Cln%ui$dE`AXB1STtv^WcTA1C437=j~ZV)RsN zGOCh#O^pBsP!-j*QeBtve^bKc5-;*HVN79YskC6EBQbQ+W}b1AyD;rL<^jzZQPXJV z(2$v;ca|O9bCOcTe14cYJ&cJBv$^c1Y0ub(dK;(H5xy=OrXhYV&ys zLK`!ivn5Z8;>7Nk0()>Kq}WD!0eFWthQ8^D5ohUJ06SH^H30r)suAY%7}HeV)<(rN zH7I4UXYl&c6u*Yy>jdzx#;=Q8cY^ta*K4=N?K4j1iyx$Dq8=%+2P!slSJjXjGWBef z$D?Y6ln*@XGb%EmvrxLmG!`VbI##4{;TW6i?}l z96u3;oBYHQq|kOZAjZKTcz$RpPt{Pk*IMa{J`w2dQevvr*K&}a;SR;O<{))HH68kU zooxF5h`^t*x#T;#^HUV)M|V-p=}o8pzh^oEKBh_iK+T7A>>$ma5V_s5Ao?ix1HOZH zM$_Y21u1vRfWG#Co+dG(dSW6i-0r5s7*+c@DEMg(MV!fRL*PwA{X~jQS6S4J#6;^M z9z4a%BOY@2W4zu`UG);@l-i}gc8qb7VAG2~Bw>K&ti;kxHQfn>&^KWOMlug*!YJit zw_{8_ogRF0nfo-t6E|XYh1S_tyS1?r-E&m8tc75ydxl$}o!Rx~0~j)k{j|caagnyK zGsS!NbQrlix?vLM@$w#QSFP4;TxXE;AwMxKU*g2nOJ2%?{`K%SKB>B}#Ko%X3cI4} z`|zWAOYXQ*8xd;@fRCuSKFI}CM2v=F{Gr~N7K{{U4Wpl_o)Sjd!m?0+-}PV*jahVH z4{qkuK+HgAYNzZc<~SMtBu|49XAUfulbTx6l`-NI#3+-RvZ|$|CJea0_*Fmy)rzF1 zaff5M3JOV07DaQUf)ulxl3TC!h&t>VoKu_x!m)W5Sw~r2$eIi-zG_l`=t6bXeKWL# zHpao&lItqayn)oGA|Tz0DYcPiXmw~~sJ$aba*nD2uv67r1K?k#8eu+&kS{xer9(8D}wEDy4X4?GWF})^Spu+VA*k@DR`emwGH(@RL zjvTh=HBx2lGduGtOV0@Y!Z87{K=cMWojEyVTtIA&k@FdT>R{x;Q=8Wg!*cL@!h2@1OSfeLV%#qSAc>LO#)ebjVLB%+CF0x^ur&B7 zP-vOsq9(crz*_}Z_L9CcPbzdFN}x%pgDv*8{ulOD0m%Xtk|C?Y+e1Ux0!V4OErW|a z4j>cEF3Rvx8ZrJQWjJ+9>W`7ia5X@BOmaQv$5Z*ZA3E)QheElu*@c&D=0qKD7bX$q z6UxuHari4?Y#242b5Zu7p$mr`JsV4(-FsQGqaTs@$u^s@YH}tPdpuXCBd~yI5~|l!;E)`@qpGO?HhA)7@MSh0A@sIp~$wi5bO=}h5}f54`mi4)}9p=X$NW)1Im0$S!rd=^R$sHU0@ny{y= znQSI|Rv<=AwbHgn@xoqPpaQ>FrozF1wl>y|@ICMFJ%bVEe311V-F`@ zWHt9g_6V7#l8af&NrI|VgHV*y=vSBrAbR(2R_9f)k9bk{vY8?k%;tu+p%BiLo}4Gs zDJo%$C+Wi(?&B7F=GDe!Mf@X|JVAy#vKJOGaNpX6rGq=H>^a&9x&`+TI{03VG0_+k z*(`?L%qh%#g;Q(l0kDG1%Tt( z`3go5yXWFFM!*da!_Z=CqpsxcFzKe#HjLTkv=k}UFPeYgIQvEuAna8Fug04IN9mOS zEoML;CS91QdFks>S0h$(u$XBHYIfdT|JzuGY7sB&pK zFw&a1B()t7#d5L9wHYvi;OBswPD$QeG7!Mq&_DezGh^NARxu-$=Fcu84b1wa}CyZOrcgU@d^NS(0|zn`9V*7USj|N002ovPDHLkV1m}kCS?Es literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/control_manager/index-detail-sort-f.html b/mrs_uav_managers/include/control_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..4fd0222d66 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
output_publisher.h +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
<unnamed>100.0 %4 / 4100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/index-detail-sort-l.html b/mrs_uav_managers/include/control_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..e26cdd23bf --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
output_publisher.h +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
<unnamed>100.0 %4 / 4100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/index-detail.html b/mrs_uav_managers/include/control_manager/index-detail.html new file mode 100644 index 0000000000..74598da6ad --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
output_publisher.h +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
<unnamed>100.0 %4 / 4100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/index-sort-f.html b/mrs_uav_managers/include/control_manager/index-sort-f.html new file mode 100644 index 0000000000..03df8066fd --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
output_publisher.h +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/index-sort-l.html b/mrs_uav_managers/include/control_manager/index-sort-l.html new file mode 100644 index 0000000000..b907568708 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
output_publisher.h +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/index.html b/mrs_uav_managers/include/control_manager/index.html new file mode 100644 index 0000000000..9a202add7a --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
output_publisher.h +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html b/mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html new file mode 100644 index 0000000000..a6252ec77c --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1314
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)114576
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.func.html b/mrs_uav_managers/include/control_manager/output_publisher.h.func.html new file mode 100644 index 0000000000..0ee04389c9 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)114576
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1314
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.frameset.html b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.frameset.html new file mode 100644 index 0000000000..137c10c3db --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html new file mode 100644 index 0000000000..1362340708 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html @@ -0,0 +1,146 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef CONTROL_MANAGER_OUTPUT_PUBLISHER
+       2             : #define CONTROL_MANAGER_OUTPUT_PUBLISHER
+       3             : 
+       4             : #include <mrs_lib/publisher_handler.h>
+       5             : 
+       6             : #include <mrs_uav_managers/controller.h>
+       7             : 
+       8             : namespace mrs_uav_managers
+       9             : {
+      10             : 
+      11             : namespace control_manager
+      12             : {
+      13             : 
+      14             : class OutputPublisher {
+      15             : 
+      16             : public:
+      17             :   OutputPublisher();
+      18             : 
+      19             :   OutputPublisher(ros::NodeHandle& nh);
+      20             : 
+      21             :   void publish(const Controller::HwApiOutputVariant& control_output);
+      22             : 
+      23             : private:
+      24             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>            ph_hw_api_actuator_cmd_;
+      25             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>        ph_hw_api_control_group_cmd_;
+      26             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>        ph_hw_api_attitude_rate_cmd_;
+      27             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>            ph_hw_api_attitude_cmd_;
+      28             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd> ph_hw_api_acceleration_hdg_rate_cmd_;
+      29             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>     ph_hw_api_acceleration_hdg_cmd_;
+      30             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>     ph_hw_api_velocity_hdg_rate_cmd_;
+      31             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>         ph_hw_api_velocity_hdg_cmd_;
+      32             :   mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>            ph_hw_api_position_cmd_;
+      33             : 
+      34             :   void publish(const mrs_msgs::HwApiActuatorCmd& msg);
+      35             :   void publish(const mrs_msgs::HwApiControlGroupCmd& msg);
+      36             :   void publish(const mrs_msgs::HwApiAttitudeRateCmd& msg);
+      37             :   void publish(const mrs_msgs::HwApiAttitudeCmd& msg);
+      38             :   void publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg);
+      39             :   void publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg);
+      40             :   void publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg);
+      41             :   void publish(const mrs_msgs::HwApiVelocityHdgCmd& msg);
+      42             :   void publish(const mrs_msgs::HwApiPositionCmd& msg);
+      43             : 
+      44             :   class PublisherVisitor {
+      45             : 
+      46             :   public:
+      47      114576 :     PublisherVisitor(OutputPublisher* obj) : obj_(obj){};
+      48             : 
+      49             :     OutputPublisher* obj_;
+      50             : 
+      51             :     template <class T>
+      52      114576 :     void operator()(const T& msg) {
+      53      114576 :       obj_->publish(msg);
+      54      114576 :     }
+      55             :   };
+      56             : };
+      57             : 
+      58             : }  // namespace control_manager
+      59             : 
+      60             : }  // namespace mrs_uav_managers
+      61             : 
+      62             : #endif  //  CONTROL_MANAGER_OUTPUT_PUBLISHER
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html new file mode 100644 index 0000000000..09b263e5e1 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html @@ -0,0 +1,36 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..fdd688bb370f7663153724647927eaa459667bca GIT binary patch literal 332 zcmeAS@N?(olHy`uVBq!ia0vp^0YGfW!VDz+4mNH9QW60^A+G=b|6c_J%iqVwzWUF= zunH&+rp|e9UJ7J$7I;J!GcfQS0b$0e+I-SL!CRg#jv*eMTc_R@JgmUO63D)Y^##MP z_qxsoW@77=HNHQc`DTrZj!g2}RPCgyMn{X24zmu;3tZ^#7E%+I9$1z2-~Hw84U=k2 z4|j*$Z0-BVwQ#LZPe@1*@1FIJ-4-|f*Lgnua4f;i^kDBLO$82VM}aqiJexoA?e=!; zP1$C<>VaG4@4cUR8vfjSmptKVZex36aP6U&;q4|#;dgdw2qk^7v)`$HtkLlNGdUR# zYu`;D4^40T_+7%T&EUnw&}9A@Oy}y>9psSfzqj9wQ#d|**7eZ4D=rjGy76rCwIcb5 X-I2 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::~AglEstimator()0
mrs_uav_managers::AglEstimator::AglEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)73
mrs_uav_managers::AglEstimator::~AglEstimator().273
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html new file mode 100644 index 0000000000..d5e5667738 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::AglEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)73
mrs_uav_managers::AglEstimator::~AglEstimator()0
mrs_uav_managers::AglEstimator::~AglEstimator().273
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.frameset.html b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.frameset.html new file mode 100644 index 0000000000..ca31d3043e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html new file mode 100644 index 0000000000..41787b3c02 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html @@ -0,0 +1,157 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_UAV_MANAGERS_AGL_ESTIMATOR_H
+       2             : #define MRS_UAV_MANAGERS_AGL_ESTIMATOR_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <Eigen/Dense>
+       9             : 
+      10             : #include <nav_msgs/Odometry.h>
+      11             : 
+      12             : #include <mrs_msgs/UavState.h>
+      13             : #include <mrs_msgs/Float64Stamped.h>
+      14             : #include <mrs_msgs/Float64ArrayStamped.h>
+      15             : #include <mrs_msgs/HwApiCapabilities.h>
+      16             : 
+      17             : #include <mrs_uav_managers/estimation_manager/estimator.h>
+      18             : #include <mrs_uav_managers/estimation_manager/support.h>
+      19             : 
+      20             : //}
+      21             : 
+      22             : namespace mrs_uav_managers
+      23             : {
+      24             : 
+      25             : namespace agl
+      26             : {
+      27             : const char type[] = "AGL";
+      28             : }
+      29             : 
+      30             : using namespace estimation_manager;
+      31             : 
+      32             : class AglEstimator : public Estimator {
+      33             : 
+      34             : protected:
+      35             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      36             : 
+      37             :   ros::NodeHandle nh_;
+      38             : 
+      39             :   mrs_msgs::Float64Stamped agl_height_;
+      40             :   mrs_msgs::Float64Stamped agl_height_init_;
+      41             :   mutable std::mutex       mtx_agl_height_;
+      42             : 
+      43             :   mrs_msgs::Float64ArrayStamped agl_height_cov_;
+      44             :   mrs_msgs::Float64ArrayStamped agl_height_cov_init_;
+      45             :   mutable std::mutex            mtx_agl_height_cov_;
+      46             : 
+      47             :   bool is_override_frame_id_ = false;
+      48             : 
+      49             : protected:
+      50             :   mutable mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>      ph_agl_height_;
+      51             :   mutable mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped> ph_agl_height_cov_;
+      52             : 
+      53             : public:
+      54          73 :   AglEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      55          73 :       : Estimator(agl::type, name, frame_id), package_name_(package_name) {
+      56          73 :   }
+      57             : 
+      58          73 :   virtual ~AglEstimator(void) {
+      59          73 :   }
+      60             : 
+      61             :   // virtual methods
+      62             :   virtual mrs_msgs::Float64Stamped getUavAglHeight() const     = 0;
+      63             :   virtual std::vector<double>      getHeightCovariance() const = 0;
+      64             : 
+      65             :   // implemented methods
+      66             :   void publishAglHeight() const;
+      67             :   void publishCovariance() const;
+      68             :   bool isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr &hw_api_capabilities) const;
+      69             : };
+      70             : 
+      71             : }  // namespace mrs_uav_managers
+      72             : 
+      73             : #endif  // MRS_UAV_MANAGERS_AGL_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..21f3af39aa --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.overview.html @@ -0,0 +1,39 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..d88c3428e27de2da87a77f1e9b45246bce5c1009 GIT binary patch literal 397 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1@!VDxMp6xjcq$C1-LR|m<|Gx?dmcNgUef6J# zVHHpuOr7)IycEdhEbxddW?X?_wfUqO7#O8JT^vI^I**2N^Bq>;m@551^bPNq zf9122k3Q-bES1dkxxB~jU(@G%i}R249^oF@8-^V(Ynvcr7WJ?T*XcTjST)6V%To!AaM zVduIYRMU7w<@9e;50Qhsq7K!Y7H0fOOAcJ-FL2X1>-ZLKhy8pP p#LM#zrZ}!(zhtL;GxNsIIKPIdz%zeOxB$bB!PC{xWt~$(697r3r%?a^ literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html new file mode 100644 index 0000000000..1e478a1892 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html @@ -0,0 +1,188 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_manager - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)1
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)494
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1003
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1062
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1314
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1314
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2329
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2458
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3776
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3779
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5168
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5172
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8028
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13564
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)14301
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)78490
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)95516
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html new file mode 100644 index 0000000000..8d7f788e34 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html @@ -0,0 +1,188 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_manager - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3776
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8028
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)494
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1314
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)78490
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3779
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1062
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2329
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1003
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)14301
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)1
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5168
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13564
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1314
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)95516
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5172
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2458
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.frameset.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.frameset.html new file mode 100644 index 0000000000..20a444da4e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html new file mode 100644 index 0000000000..ed95917cc8 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html @@ -0,0 +1,380 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_manager - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef CONTROL_MANAGER_COMMON_H
+       2             : #define CONTROL_MANAGER_COMMON_H
+       3             : 
+       4             : #include <ros/ros.h>
+       5             : 
+       6             : #include <vector>
+       7             : #include <string>
+       8             : #include <optional>
+       9             : #include <variant>
+      10             : 
+      11             : #include <mrs_lib/attitude_converter.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : 
+      14             : #include <mrs_uav_managers/control_manager/common_handlers.h>
+      15             : #include <mrs_uav_managers/controller.h>
+      16             : 
+      17             : #include <mrs_msgs/TrackerCommand.h>
+      18             : #include <mrs_msgs/UavState.h>
+      19             : #include <mrs_msgs/VelocityReference.h>
+      20             : 
+      21             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      22             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      23             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      24             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      25             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      26             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      27             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      28             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      29             : #include <mrs_msgs/HwApiPositionCmd.h>
+      30             : 
+      31             : #include <mrs_msgs/HwApiCapabilities.h>
+      32             : 
+      33             : #include <nav_msgs/Odometry.h>
+      34             : 
+      35             : namespace mrs_uav_managers
+      36             : {
+      37             : 
+      38             : namespace control_manager
+      39             : {
+      40             : 
+      41             : enum CONTROL_OUTPUT
+      42             : {
+      43             :   ACTUATORS_CMD,
+      44             :   CONTROL_GROUP,
+      45             :   ATTITUDE_RATE,
+      46             :   ATTITUDE,
+      47             :   ACCELERATION_HDG_RATE,
+      48             :   ACCELERATION_HDG,
+      49             :   VELOCITY_HDG_RATE,
+      50             :   VELOCITY_HDG,
+      51             :   POSITION
+      52             : };
+      53             : 
+      54             : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs);
+      55             : 
+      56             : CONTROL_OUTPUT getHighestOuput(const ControlOutputModalities_t& outputs);
+      57             : 
+      58             : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec);
+      59             : 
+      60             : // checks for invalid values in the result from trackers
+      61             : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name);
+      62             : 
+      63             : // checks for invalid messages in/out
+      64             : bool validateOdometry(const nav_msgs::Odometry& msg, const std::string& node_name, const std::string& var_name);
+      65             : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_nam);
+      66             : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name);
+      67             : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name);
+      68             : 
+      69             : std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name, const std::string& platform_config,
+      70             :                                                                 const std::string& custom_config);
+      71             : 
+      72             : // translates the channel values to desired range
+      73             : double RCChannelToRange(double rc_value, double range, double deadband);
+      74             : 
+      75             : /* throttle extraction //{ */
+      76             : 
+      77             : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output);
+      78             : 
+      79             : struct HwApiCmdExtractThrottleVisitor
+      80             : {
+      81        5168 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      82             : 
+      83        5168 :     if (msg.motors.size() == 0) {
+      84           0 :       return std::nullopt;
+      85             :     }
+      86             : 
+      87        5168 :     double throttle = 0;
+      88             : 
+      89       25840 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      90       20672 :       throttle += msg.motors.at(i);
+      91             :     };
+      92             : 
+      93        5168 :     throttle /= msg.motors.size();
+      94             : 
+      95        5168 :     return throttle;
+      96             :   }
+      97        5172 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      98        5172 :     return msg.throttle;
+      99             :   }
+     100       13564 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+     101       13564 :     return msg.throttle;
+     102             :   }
+     103       95516 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+     104       95516 :     return msg.throttle;
+     105             :   }
+     106        1003 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+     107        1003 :     return std::nullopt;
+     108             :   }
+     109        1062 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+     110        1062 :     return std::nullopt;
+     111             :   }
+     112        2458 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+     113        2458 :     return std::nullopt;
+     114             :   }
+     115        1314 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+     116        1314 :     return std::nullopt;
+     117             :   }
+     118         494 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+     119         494 :     return std::nullopt;
+     120             :   }
+     121             : };
+     122             : 
+     123             : //}
+     124             : 
+     125             : /* control output validation //{ */
+     126             : 
+     127             : bool validateControlOutput(const Controller::ControlOutput& control_output, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     128             :                            const std::string& var_name);
+     129             : 
+     130             : // validation of hw api messages
+     131             : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name);
+     132             : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name);
+     133             : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name);
+     134             : bool validateHwApiAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd& msg, const std::string& node_name, const std::string& var_name);
+     135             : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name);
+     136             : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name);
+     137             : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name);
+     138             : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name);
+     139             : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name);
+     140             : 
+     141             : struct HwApiValidateVisitor
+     142             : {
+     143        3776 :   bool operator()(const mrs_msgs::HwApiActuatorCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     144             :                   const std::string& var_name) {
+     145             : 
+     146        3776 :     if (!output_modalities.actuators) {
+     147           0 :       ROS_ERROR("[%s]: The controller returned an output modality (actuator cmd) that is not supported by the hardware API", node_name.c_str());
+     148           0 :       return false;
+     149             :     }
+     150             : 
+     151        3776 :     return validateHwApiActuatorCmd(msg, node_name, var_name);
+     152             :   }
+     153        3779 :   bool operator()(const mrs_msgs::HwApiControlGroupCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     154             :                   const std::string& var_name) {
+     155             : 
+     156        3779 :     if (!output_modalities.control_group) {
+     157           0 :       ROS_ERROR("[%s]: The controller returned an output modality (control group cmd) that is not supported by the hardware API", node_name.c_str());
+     158           0 :       return false;
+     159             :     }
+     160             : 
+     161        3779 :     return validateHwApiControlGroupCmd(msg, node_name, var_name);
+     162             :   }
+     163        8028 :   bool operator()(const mrs_msgs::HwApiAttitudeCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     164             :                   const std::string& var_name) {
+     165             : 
+     166        8028 :     if (!output_modalities.attitude) {
+     167           0 :       ROS_ERROR("[%s]: The controller returned an output modality (attitude cmd) that is not supported by the hardware API", node_name.c_str());
+     168           0 :       return false;
+     169             :     }
+     170             : 
+     171        8028 :     return validateHwApiAttitudeCmd(msg, node_name, var_name);
+     172             :   }
+     173       78490 :   bool operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     174             :                   const std::string& var_name) {
+     175             : 
+     176       78490 :     if (!output_modalities.attitude_rate) {
+     177           0 :       ROS_ERROR("[%s]: The controller returned an output modality (attitude rate cmd) that is not supported by the hardware API", node_name.c_str());
+     178           0 :       return false;
+     179             :     }
+     180             : 
+     181       78490 :     return validateHwApiAttitudeRateCmd(msg, node_name, var_name);
+     182             :   }
+     183        1003 :   bool operator()(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     184             :                   const std::string& var_name) {
+     185             : 
+     186        1003 :     if (!output_modalities.acceleration_hdg_rate) {
+     187           0 :       ROS_ERROR("[%s]: The controller returned an output modality (acceleration+hdg rate cmd) that is not supported by the hardware API", node_name.c_str());
+     188           0 :       return false;
+     189             :     }
+     190             : 
+     191        1003 :     return validateHwApiAccelerationHdgRateCmd(msg, node_name, var_name);
+     192             :   }
+     193        1062 :   bool operator()(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     194             :                   const std::string& var_name) {
+     195             : 
+     196        1062 :     if (!output_modalities.acceleration_hdg) {
+     197           0 :       ROS_ERROR("[%s]: The controller returned an output modality (acceleration+hdg cmd) that is not supported by the hardware API", node_name.c_str());
+     198           0 :       return false;
+     199             :     }
+     200             : 
+     201        1062 :     return validateHwApiAccelerationHdgCmd(msg, node_name, var_name);
+     202             :   }
+     203        2329 :   bool operator()(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     204             :                   const std::string& var_name) {
+     205             : 
+     206        2329 :     if (!output_modalities.velocity_hdg_rate) {
+     207           0 :       ROS_ERROR("[%s]: The controller returned an output modality (velocity+hdg rate cmd) that is not supported by the hardware API", node_name.c_str());
+     208           0 :       return false;
+     209             :     }
+     210             : 
+     211        2329 :     return validateHwApiVelocityHdgRateCmd(msg, node_name, var_name);
+     212             :   }
+     213        1314 :   bool operator()(const mrs_msgs::HwApiVelocityHdgCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     214             :                   const std::string& var_name) {
+     215             : 
+     216        1314 :     if (!output_modalities.velocity_hdg) {
+     217           0 :       ROS_ERROR("[%s]: The controller returned an output modality (velocity+hdg cmd) that is not supported by the hardware API", node_name.c_str());
+     218           0 :       return false;
+     219             :     }
+     220             : 
+     221        1314 :     return validateHwApiVelocityHdgCmd(msg, node_name, var_name);
+     222             :   }
+     223         494 :   bool operator()(const mrs_msgs::HwApiPositionCmd& msg, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     224             :                   const std::string& var_name) {
+     225             : 
+     226         494 :     if (!output_modalities.position) {
+     227           0 :       ROS_ERROR("[%s]: The controller returned an output modality (position cmd) that is not supported by the hardware API", node_name.c_str());
+     228           0 :       return false;
+     229             :     }
+     230             : 
+     231         494 :     return validateHwApiPositionCmd(msg, node_name, var_name);
+     232             :   }
+     233             : };
+     234             : 
+     235             : //}
+     236             : 
+     237             : /* control output initialization //{ */
+     238             : 
+     239             : Controller::HwApiOutputVariant initializeDefaultOutput(const ControlOutputModalities_t& possible_outputs, const mrs_msgs::UavState& uav_state,
+     240             :                                                        const double& min_throttle, const double& n_motors);
+     241             : 
+     242             : void initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg, const double& min_throttle, const double& n_motors);
+     243             : void initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg, const double& min_throttle);
+     244             : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle);
+     245             : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle);
+     246             : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state);
+     247             : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state);
+     248             : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state);
+     249             : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state);
+     250             : void initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state);
+     251             : 
+     252             : struct HwApiInitializeVisitor
+     253             : {
+     254           0 :   void operator()(mrs_msgs::HwApiActuatorCmd& msg, [[maybe_unused]] const mrs_msgs::UavState& uav_state, const double& min_throttle, const double& n_motors) {
+     255           0 :     initializeHwApiCmd(msg, min_throttle, n_motors);
+     256           0 :   }
+     257           0 :   void operator()(mrs_msgs::HwApiControlGroupCmd& msg, [[maybe_unused]] const mrs_msgs::UavState& uav_state, const double& min_throttle,
+     258             :                   [[maybe_unused]] const double& n_motors) {
+     259           0 :     initializeHwApiCmd(msg, min_throttle);
+     260           0 :   }
+     261           0 :   void operator()(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle, [[maybe_unused]] const double& n_motors) {
+     262           0 :     initializeHwApiCmd(msg, uav_state, min_throttle);
+     263           0 :   }
+     264       14301 :   void operator()(mrs_msgs::HwApiAttitudeRateCmd& msg, [[maybe_unused]] const mrs_msgs::UavState& uav_state, const double& min_throttle,
+     265             :                   [[maybe_unused]] const double& n_motors) {
+     266       14301 :     initializeHwApiCmd(msg, min_throttle);
+     267       14301 :   }
+     268           0 :   void operator()(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     269             :                   [[maybe_unused]] const double& n_motors) {
+     270           0 :     initializeHwApiCmd(msg, uav_state);
+     271           0 :   }
+     272           0 :   void operator()(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     273             :                   [[maybe_unused]] const double& n_motors) {
+     274           0 :     initializeHwApiCmd(msg, uav_state);
+     275           0 :   }
+     276           1 :   void operator()(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     277             :                   [[maybe_unused]] const double& n_motors) {
+     278           1 :     initializeHwApiCmd(msg, uav_state);
+     279           1 :   }
+     280           0 :   void operator()(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     281             :                   [[maybe_unused]] const double& n_motors) {
+     282           0 :     initializeHwApiCmd(msg, uav_state);
+     283           0 :   }
+     284           0 :   void operator()(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     285             :                   [[maybe_unused]] const double& n_motors) {
+     286           0 :     initializeHwApiCmd(msg, uav_state);
+     287           0 :   }
+     288             : };
+     289             : 
+     290             : //}
+     291             : 
+     292             : }  // namespace control_manager
+     293             : 
+     294             : }  // namespace mrs_uav_managers
+     295             : 
+     296             : #endif  // CONTROL_MANAGER_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html new file mode 100644 index 0000000000..29005b23a1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html @@ -0,0 +1,94 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..27063a66007e8a8a221495f64c99bbed3983a328 GIT binary patch literal 947 zcmV;k15EshP)~ejb`Kt$kDKqYg2*?t^rm`YxWY91nC_2J*#I`=bmf<99W}m4y>r8X2jrFML z7}0H0Hl-rtC)s^5JDIivIYW{2Gzl>XqP&4#!8FUy!#HqRmUUfUtCl4%Z=fpN20*e1 zVZ)rJc_$cp)RWft4q1ZuXPIi$qv}RGGd>@UG}Coa1Is!6RZ-h?*#qXXAOY^?qsd5M zx@^3sX*U(FH+^Z;+Znv@GyOB&VETk+xciSTy|%KRO+dU{r_HtbA3ha!f(J)#>!fVI zTzXL%w-oW2@dxQhlMb6B9qB0=jij%DP#|KY37AHQ29t}rB70yvUjWv0doou5Bv^8M z2jGDEo=oO7SzudeZKAcqwuM?=raM&nSlMP#*-O5E-02R_>>q(hKdJ}nW0skSn@gC% zxuG%hdLA{hVvq3Fm`CntWRXdjjExjA2ZVb z5V(w)w4!}8GZ8Oe%}iR)sLZH;!wlf%GG?$I;Cq=#c=`2a&tRslH~YT#Jw4v|lwTD1 z(~=JR_NVqsuk05{ejm_mHWfZq3ZNZ;a0_zB^f-du;W7|zBKH6SSWPfBNdfnK^FVU) zEpFQkit<;LDJz37JzxpYf8&xut0+sxbZUHXkND!J5gPRhs#)qIwjJ2&qjD~6{$Qr< zeHCe>nihDwZr7vja0(VAS8gS;EDb`0YQlmHfz;BRqtJw}90^Snq65~RW}%1o`k3Q7 z>I&Pq#@~@Pa~+PNX$$Wf$BOBCn~$aM69OQ8Rp{Q%aK`OAwYSh5)Y#`6_JQ*8^&hU3 VN}7e(vrqs4002ovPDHLkV1g}Szw`hA literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..b058664536 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
<unnamed>58.3 %56 / 9674.1 %20 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..1d1f5d13ef --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
<unnamed>58.3 %56 / 9674.1 %20 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html new file mode 100644 index 0000000000..20ef2d629c --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
<unnamed>58.3 %56 / 9674.1 %20 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html new file mode 100644 index 0000000000..538596f977 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html new file mode 100644 index 0000000000..e5d3427123 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html new file mode 100644 index 0000000000..fffbcf4e88 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:569658.3 %
Date:2024-04-25 05:42:57Functions:202774.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
58.3%58.3%
+ 		58.3 %56 / 9674.1 %20 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html new file mode 100644 index 0000000000..6fb0d9d9e5 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-25 05:42:57Functions:1250.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Controller::~Controller()0
mrs_uav_managers::Controller::~Controller().2470
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html new file mode 100644 index 0000000000..50015f17ad --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-25 05:42:57Functions:1250.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Controller::~Controller()0
mrs_uav_managers::Controller::~Controller().2470
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.frameset.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.frameset.html new file mode 100644 index 0000000000..34044d941d --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html new file mode 100644 index 0000000000..84082493d0 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-25 05:42:57Functions:1250.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_UAV_CONTROLLER_H
+       2             : #define MRS_UAV_CONTROLLER_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <mrs_uav_managers/control_manager/common_handlers.h>
+       9             : #include <mrs_uav_managers/control_manager/private_handlers.h>
+      10             : 
+      11             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      12             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      13             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      14             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      15             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      16             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      17             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      18             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      19             : #include <mrs_msgs/HwApiPositionCmd.h>
+      20             : 
+      21             : #include <mrs_msgs/ControllerDiagnostics.h>
+      22             : #include <mrs_msgs/ControllerStatus.h>
+      23             : #include <mrs_msgs/TrackerCommand.h>
+      24             : #include <mrs_msgs/UavState.h>
+      25             : 
+      26             : #include <mrs_msgs/DynamicsConstraintsSrv.h>
+      27             : #include <mrs_msgs/DynamicsConstraintsSrvRequest.h>
+      28             : #include <mrs_msgs/DynamicsConstraintsSrvResponse.h>
+      29             : 
+      30             : //}
+      31             : 
+      32             : namespace mrs_uav_managers
+      33             : {
+      34             : 
+      35             : class Controller {
+      36             : public:
+      37             :   typedef std::variant<mrs_msgs::HwApiActuatorCmd, mrs_msgs::HwApiControlGroupCmd, mrs_msgs::HwApiAttitudeRateCmd, mrs_msgs::HwApiAttitudeCmd,
+      38             :                        mrs_msgs::HwApiAccelerationHdgRateCmd, mrs_msgs::HwApiAccelerationHdgCmd, mrs_msgs::HwApiVelocityHdgRateCmd,
+      39             :                        mrs_msgs::HwApiVelocityHdgCmd, mrs_msgs::HwApiPositionCmd>
+      40             :       HwApiOutputVariant;
+      41             : 
+      42             :   typedef struct
+      43             :   {
+      44             :     std::optional<HwApiOutputVariant> control_output;
+      45             :     mrs_msgs::ControllerDiagnostics   diagnostics;
+      46             : 
+      47             :     /**
+      48             :      * @brief Desired orientation is used for checking the orientation control error.
+      49             :      *        This variable is optional, fill it in if you know it.
+      50             :      */
+      51             :     std::optional<Eigen::Quaterniond> desired_orientation;
+      52             : 
+      53             :     /**
+      54             :      * @brief Desired unbiased acceleration is used by the MRS odometry as control input.
+      55             :      *        This variable is optional, fill it in if you know it.
+      56             :      */
+      57             :     std::optional<Eigen::Vector3d> desired_unbiased_acceleration;
+      58             : 
+      59             :     /**
+      60             :      * @brief Desired heading rate caused by the controllers control action.
+      61             :      *        This variable is optional, fill it in if you know it.
+      62             :      */
+      63             :     std::optional<double> desired_heading_rate;
+      64             :   } ControlOutput;
+      65             : 
+      66             :   /**
+      67             :    * @brief Initializes the controller. It is called once for every controller. The runtime is not limited.
+      68             :    *
+      69             :    * @param nh the node handle of the ControlManager
+      70             :    * @param name of the controller for distinguishing multiple running instances of the same code
+      71             :    * @param name_space the parameter namespace of the controller, can be used during initialization of the private node handle
+      72             :    * @param common_handlers handlers shared between trackers and controllers
+      73             :    * @param private_handlers handlers provided individually to each controller
+      74             :    *
+      75             :    * @return true if success
+      76             :    */
+      77             :   virtual bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      78             :                           std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) = 0;
+      79             : 
+      80             :   /**
+      81             :    * @brief It is called before the controller output will be required and used. Should not take much time (within miliseconds).
+      82             :    *
+      83             :    * @param last_attitude_cmd the last command produced by the last active controller. Should be used as an initial condition, e.g., for re-initializing
+      84             :    * integrators and estimators.
+      85             :    *
+      86             :    * @return true if success
+      87             :    */
+      88             :   virtual bool activate(const ControlOutput &last_control_output) = 0;
+      89             : 
+      90             :   /**
+      91             :    * @brief is called when this controller's output is no longer needed. However, it can be activated later.
+      92             :    */
+      93             :   virtual void deactivate(void) = 0;
+      94             : 
+      95             :   /**
+      96             :    * @brief It may be called to reset the controllers disturbance estimators.
+      97             :    */
+      98             :   virtual void resetDisturbanceEstimators(void) = 0;
+      99             : 
+     100             :   /**
+     101             :    * @brief This method is called in the main feedback control loop when your controller is NOT active. You can use this to validate your results without endangering the drone.
+     102             :    *        The method is called even before the flight with just the uav_state being supplied.
+     103             :    *
+     104             :    * @param uav_state current estimated state of the UAV dynamics
+     105             :    * @param tracker_command current required control reference (is optional)
+     106             :    */
+     107             :   virtual void updateInactive(const mrs_msgs::UavState &uav_state, const std::optional<mrs_msgs::TrackerCommand> &tracker_command) = 0;
+     108             : 
+     109             :   /**
+     110             :    * @brief This method is called in the main feedback control loop when your controller IS active and when it is supposed to produce a control output.
+     111             :    *
+     112             :    * @param uav_state current estimated state of the UAV dynamics
+     113             :    * @param tracker_command current required control reference
+     114             :    *
+     115             :    * @return produced control output
+     116             :    */
+     117             :   virtual ControlOutput updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) = 0;
+     118             : 
+     119             :   /**
+     120             :    * @brief A request for the controller's status.
+     121             :    *
+     122             :    * @return the controller's status
+     123             :    */
+     124             :   virtual const mrs_msgs::ControllerStatus getStatus() = 0;
+     125             : 
+     126             :   /**
+     127             :    * @brief It is called during every switch of reference frames of the UAV state estimate.
+     128             :    * The controller should recalculate its internal states from old the frame to the new one.
+     129             :    *
+     130             :    * @param new_uav_state the new UavState which will come in the next update()
+     131             :    */
+     132             :   virtual void switchOdometrySource(const mrs_msgs::UavState &new_uav_state) = 0;
+     133             : 
+     134             :   /**
+     135             :    * @brief Request for setting new constraints.
+     136             :    *
+     137             :    * @param constraints to be set
+     138             :    *
+     139             :    * @return a service response
+     140             :    */
+     141             :   virtual const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) = 0;
+     142             : 
+     143         470 :   virtual ~Controller() = default;
+     144             : };
+     145             : 
+     146             : }  // namespace mrs_uav_managers
+     147             : 
+     148             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html new file mode 100644 index 0000000000..7ccc3a2839 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3aa437cffc6a4bac8d6472ee5f8996438a7ffd35 GIT binary patch literal 681 zcmV;a0#^NrP)gyv~hF-^&dW?vH8lQFyr0yFtbG*21IvQ>PFm5wzv~q9s(U*D?$PkPb*mg9K z4;`1wwTa!aQn#n9$1^8R4EK&++_~G$Y7^}l1fj>V&qK*=9V{9pxp(yE8zD8n&L7kKa*6i+*$L2+n!s;Qt zWF?Y#<*tX-BVbX5&}fQ9abk54F_QX+6k<>0_iOxZEXoWOfTDC|i(szm@qstVFc_eX z((J0{GcAMJAVebuH?H!-vd*kO-exr{K^E;Rr%15l!zD`*xd6!R$#h<%{YC~}%o`2Y zF_4q^ZtTtxf79?l#N9r&5hJPP#F|~*gUnc*v;!8oqQ#i0K;)WWsvz1Pkm84qvG(+D zVkB92j8nwb(^YqYUagSMLXiJv9Z|i>lu*k8DdmHzvH*go2mfisL)0D + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Estimator::~Estimator()0
mrs_uav_managers::Estimator::Estimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)543
mrs_uav_managers::Estimator::~Estimator().2543
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html new file mode 100644 index 0000000000..ff7ceb40b2 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Estimator::Estimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)543
mrs_uav_managers::Estimator::~Estimator()0
mrs_uav_managers::Estimator::~Estimator().2543
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.frameset.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.frameset.html new file mode 100644 index 0000000000..37d4a69ab9 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html new file mode 100644 index 0000000000..3268f3aa13 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html @@ -0,0 +1,195 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_UAV_MANAGERS_ESTIMATION_MANAGER_ESTIMATOR_H
+       2             : #define MRS_UAV_MANAGERS_ESTIMATION_MANAGER_ESTIMATOR_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <Eigen/Dense>
+       9             : 
+      10             : #include <nav_msgs/Odometry.h>
+      11             : 
+      12             : #include <sensor_msgs/Imu.h>
+      13             : 
+      14             : #include <mrs_msgs/UavState.h>
+      15             : #include <mrs_msgs/EstimatorInput.h>
+      16             : #include <mrs_msgs/EstimatorDiagnostics.h>
+      17             : 
+      18             : #include <mrs_lib/publisher_handler.h>
+      19             : #include <mrs_lib/attitude_converter.h>
+      20             : #include <mrs_lib/param_loader.h>
+      21             : #include <mrs_lib/mutex.h>
+      22             : 
+      23             : 
+      24             : #include <mrs_uav_managers/estimation_manager/types.h>
+      25             : #include <mrs_uav_managers/estimation_manager/support.h>
+      26             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      27             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      28             : 
+      29             : //}
+      30             : 
+      31             : namespace mrs_uav_managers
+      32             : {
+      33             : 
+      34             : /* using namespace estimation_manager; */
+      35             : 
+      36             : using namespace mrs_uav_managers::estimation_manager;
+      37             : 
+      38             : class Estimator {
+      39             : 
+      40             : protected:
+      41             :   mutable mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics> ph_diagnostics_;
+      42             : 
+      43             :   const std::string type_;
+      44             :   const std::string name_;
+      45             :   const std::string package_name_;
+      46             : 
+      47             :   std::string frame_id_;  // cannot be constant - must remain overridable by loaded parameter
+      48             :   std::string ns_frame_id_;
+      49             : 
+      50             :   std::shared_ptr<CommonHandlers_t>  ch_;
+      51             :   std::shared_ptr<PrivateHandlers_t> ph_;
+      52             : 
+      53             :   double max_flight_z_ = -1.0;
+      54             : 
+      55             :   std::atomic_bool is_mitigating_jump_ = false;
+      56             : 
+      57             : private:
+      58             :   SMStates_t         previous_sm_state_ = SMStates_t::UNINITIALIZED_STATE;
+      59             :   SMStates_t         current_sm_state_  = SMStates_t::UNINITIALIZED_STATE;
+      60             :   mutable std::mutex mutex_current_state_;
+      61             : 
+      62             : protected:
+      63         543 :   Estimator(const std::string &type, const std::string &name, const std::string &frame_id) : type_(type), name_(name), frame_id_(frame_id) {
+      64         543 :   }
+      65             : 
+      66         543 :   virtual ~Estimator(void) {
+      67         543 :   }
+      68             : 
+      69             : public:
+      70             :   // virtual methods
+      71             :   virtual void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) = 0;
+      72             :   virtual bool start(void)                                                                                                                = 0;
+      73             :   virtual bool pause(void)                                                                                                                = 0;
+      74             :   virtual bool reset(void)                                                                                                                = 0;
+      75             : 
+      76             :   // implemented methods
+      77             :   // access methods
+      78             :   std::string getName(void) const;
+      79             :   std::string getPrintName(void) const;
+      80             :   std::string getType(void) const;
+      81             :   std::string getFrameId(void) const;
+      82             :   double      getMaxFlightZ(void) const;
+      83             :   std::string getSmStateString(const SMStates_t &state) const;
+      84             :   std::string getCurrentSmStateString(void) const;
+      85             :   SMStates_t  getCurrentSmState() const;
+      86             : 
+      87             :   void setCurrentSmState(const SMStates_t &new_state);
+      88             : 
+      89             :   bool isMitigatingJump();
+      90             : 
+      91             :   // state machine methods
+      92             :   bool changeState(SMStates_t new_state);
+      93             :   bool isInState(const SMStates_t &state_in) const;
+      94             :   bool isInitialized() const;
+      95             :   bool isReady() const;
+      96             :   bool isStarted() const;
+      97             :   bool isRunning() const;
+      98             :   bool isStopped() const;
+      99             :   bool isError() const;
+     100             : 
+     101             :   void publishDiagnostics() const;
+     102             : 
+     103             :   tf2::Vector3          getAccGlobal(const sensor_msgs::Imu::ConstPtr &input_msg, const double hdg);
+     104             :   tf2::Vector3          getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr &input_msg, const double hdg);
+     105             :   tf2::Vector3          getAccGlobal(const geometry_msgs::Vector3Stamped &acc_stamped, const double hdg);
+     106             :   std::optional<double> getHeadingRate(const nav_msgs::OdometryConstPtr &odom_msg);
+     107             : };
+     108             : 
+     109             : }  // namespace mrs_uav_managers
+     110             : 
+     111             : #endif  // MRS_UAV_MANAGERS_ESTIMATION_MANAGER_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html new file mode 100644 index 0000000000..1a1e94470f --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html @@ -0,0 +1,48 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..77fe2beffc5729db0f0ec855eec111008d96cc88 GIT binary patch literal 506 zcmVa008j`=Fmn2hkQk9Ahg7z^spw9N-=}-rXW^kOaEvTlo0FNfIZNoT@1~LKB3<`J! z?pJ}gg=3mSIbx#c_7C;h?A!KjK= zE@eohrWcWJ=vP1iI&F(Cz%Z$M!ZdKlb>0O4ejN!>Y%Vwolzn+?CRE?COJoHMDj1vD zocBVL!u27$>-$f7(ZyYV2-kg?EZDFwSlxb|%|Lh8_AQlG1PsBXB^2$D?%UH42hEB; zpxLt6dfugIEu&ciQa(%|4MEXSTB*K?(a)1=Q3(6PGGaabO(%KnZn@uKO9pHhm z_kW7#gtW{vF`3*X7a*JQXvqaQ%iiY + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-25 05:42:57Functions:171894.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
support.h +
97.1%97.1%
+ 		97.1 %102 / 105100.0 %15 / 15
<unnamed>97.1 %102 / 105100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..2d253df39e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-25 05:42:57Functions:171894.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
support.h +
97.1%97.1%
+ 		97.1 %102 / 105100.0 %15 / 15
<unnamed>97.1 %102 / 105100.0 %15 / 15
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html new file mode 100644 index 0000000000..970a4bd494 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-25 05:42:57Functions:171894.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
support.h +
97.1%97.1%
+ 		97.1 %102 / 105100.0 %15 / 15
<unnamed>97.1 %102 / 105100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html new file mode 100644 index 0000000000..7ae724110c --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-25 05:42:57Functions:171894.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
support.h +
97.1%97.1%
+ 		97.1 %102 / 105100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html new file mode 100644 index 0000000000..0811b6a414 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-25 05:42:57Functions:171894.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
support.h +
97.1%97.1%
+ 		97.1 %102 / 105100.0 %15 / 15
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html new file mode 100644 index 0000000000..3c659ad1f3 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-25 05:42:57Functions:171894.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
support.h +
97.1%97.1%
+ 		97.1 %102 / 105100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html new file mode 100644 index 0000000000..c07d6ef589 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-04-25 05:42:57Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)5
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10
mrs_uav_managers::estimation_manager::Support::isStringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)294
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)376
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2139
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)158584
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)284317
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)332230
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)343241
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)344289
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)366427
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)692345
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)703002
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)1035758
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1134113
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html new file mode 100644 index 0000000000..30c7427887 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-04-25 05:42:57Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)332230
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)5
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)703002
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)1035758
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)376
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2139
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)344289
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)158584
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)692345
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)284317
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)343241
mrs_uav_managers::estimation_manager::Support::isStringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)294
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)366427
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1134113
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.frameset.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.frameset.html new file mode 100644 index 0000000000..e8f1cf866e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html new file mode 100644 index 0000000000..528a963552 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html @@ -0,0 +1,405 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-04-25 05:42:57Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef ESTIMATION_MANAGER_SUPPORT_H
+       3             : #define ESTIMATION_MANAGER_SUPPORT_H
+       4             : 
+       5             : /* includes //{ */
+       6             : 
+       7             : #include <string.h>
+       8             : 
+       9             : #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+      10             : 
+      11             : #include <geometry_msgs/TransformStamped.h>
+      12             : #include <geometry_msgs/Pose.h>
+      13             : #include <geometry_msgs/PointStamped.h>
+      14             : 
+      15             : #include <nav_msgs/Odometry.h>
+      16             : 
+      17             : #include <mrs_msgs/UavState.h>
+      18             : 
+      19             : #include <mrs_lib/attitude_converter.h>
+      20             : #include <mrs_lib/transformer.h>
+      21             : 
+      22             : //}
+      23             : 
+      24             : namespace mrs_uav_managers
+      25             : {
+      26             : 
+      27             : namespace estimation_manager
+      28             : {
+      29             : 
+      30             : /*//{ class Support */
+      31             : class Support {
+      32             : 
+      33             : public:
+      34             : 
+      35             :   const static inline std::string waiting_for_string = "\033[0;36mWAITING FOR:\033[0m";
+      36             : 
+      37             :   /*//{ toSnakeCase() */
+      38        2139 :   static std::string toSnakeCase(const std::string& str_in) {
+      39             : 
+      40        2139 :     std::string str(1, tolower(str_in[0]));
+      41             : 
+      42       33599 :     for (auto it = str_in.begin() + 1; it != str_in.end(); ++it) {
+      43       31460 :       if (isupper(*it) && *(it - 1) != '_' && islower(*(it - 1))) {
+      44        1767 :         str += "_";
+      45             :       }
+      46       31460 :       str += *it;
+      47             :     }
+      48             : 
+      49        2139 :     std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+      50             : 
+      51        2139 :     return str;
+      52             :   }
+      53             :   /*//}*/
+      54             : 
+      55             : /* toLowercase() //{ */
+      56             : 
+      57         376 : static std::string toLowercase(const std::string str_in) {
+      58         376 :   std::string str_out = str_in;
+      59         376 :   std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::tolower);
+      60         376 :   return str_out;
+      61             : }
+      62             : 
+      63             : //}
+      64             : 
+      65             : /* toUppercase() //{ */
+      66             : 
+      67             : static std::string toUppercase(const std::string str_in) {
+      68             :   std::string str_out = str_in;
+      69             :   std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::toupper);
+      70             :   return str_out;
+      71             : }
+      72             : 
+      73             : //}
+      74             : 
+      75             :   /*//{ stateCovToString() */
+      76             :   template <typename StateCov>
+      77             :   static std::string stateCovToString(const StateCov& sc) {
+      78             :     std::stringstream ss;
+      79             :     ss << "State:\n";
+      80             :     for (int i = 0; i < sc.x.rows(); i++) {
+      81             :       for (int j = 0; j < sc.x.cols(); j++) {
+      82             :         ss << sc.x(i, j) << " ";
+      83             :       }
+      84             :       ss << "\n";
+      85             :     }
+      86             :     ss << "Cov:\n";
+      87             :     for (int i = 0; i < sc.P.rows(); i++) {
+      88             :       for (int j = 0; j < sc.P.cols(); j++) {
+      89             :         ss << sc.P(i, j) << " ";
+      90             :       }
+      91             :       ss << "\n";
+      92             :     }
+      93             :     return ss.str();
+      94             :   }
+      95             :   /*//}*/
+      96             : 
+      97             :   /* //{ rotateVecByHdg() */
+      98      284317 :   static tf2::Vector3 rotateVecByHdg(const geometry_msgs::Vector3& vec_in, const double hdg_in) {
+      99             : 
+     100      284317 :     const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
+     101             : 
+     102      280066 :     const tf2::Vector3 vec_tf2(vec_in.x, vec_in.y, vec_in.z);
+     103             : 
+     104      279784 :     const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
+     105             : 
+     106      557746 :     return vec_rotated;
+     107             :   }
+     108             :   //}
+     109             : 
+     110             :   /* noNans() //{ */
+     111     1134113 :   static bool noNans(const geometry_msgs::TransformStamped& tf) {
+     112             : 
+     113     3399281 :     return (std::isfinite(tf.transform.rotation.x) && std::isfinite(tf.transform.rotation.y) && std::isfinite(tf.transform.rotation.z) &&
+     114     4530485 :             std::isfinite(tf.transform.rotation.w) && std::isfinite(tf.transform.translation.x) && std::isfinite(tf.transform.translation.y) &&
+     115     2265603 :             std::isfinite(tf.transform.translation.z));
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* noNans() //{ */
+     120      366427 :   static bool noNans(const geometry_msgs::Quaternion& q) {
+     121             : 
+     122      366427 :     return (std::isfinite(q.x) && std::isfinite(q.y) && std::isfinite(q.z) && std::isfinite(q.w));
+     123             :   }
+     124             :   //}
+     125             : 
+     126             :   /* isZeroQuaternion() //{ */
+     127             :   static bool isZeroQuaternion(const geometry_msgs::Quaternion& q) {
+     128             : 
+     129             :     return (q.x == 0 && q.y == 0 && q.z == 0 && q.w == 0);
+     130             :   }
+     131             :   //}
+     132             : 
+     133             :   /* tf2FromPose() //{ */
+     134             : 
+     135     1035758 :   static tf2::Transform tf2FromPose(const geometry_msgs::Pose& pose_in) {
+     136             : 
+     137     1035758 :     tf2::Vector3 position(pose_in.position.x, pose_in.position.y, pose_in.position.z);
+     138             : 
+     139     1035384 :     tf2::Quaternion q;
+     140     1035510 :     tf2::fromMsg(pose_in.orientation, q);
+     141             : 
+     142     1035295 :     tf2::Transform tf_out(q, position);
+     143             : 
+     144     2066652 :     return tf_out;
+     145             :   }
+     146             : 
+     147             :   //}
+     148             : 
+     149             :   /* poseFromTf2() //{ */
+     150             : 
+     151      703002 :   static geometry_msgs::Pose poseFromTf2(const tf2::Transform& tf_in) {
+     152             : 
+     153      703002 :     geometry_msgs::Pose pose_out;
+     154      701433 :     pose_out.position.x = tf_in.getOrigin().getX();
+     155      701417 :     pose_out.position.y = tf_in.getOrigin().getY();
+     156      702027 :     pose_out.position.z = tf_in.getOrigin().getZ();
+     157             : 
+     158      702650 :     pose_out.orientation = tf2::toMsg(tf_in.getRotation());
+     159             : 
+     160      702819 :     return pose_out;
+     161             :   }
+     162             : 
+     163             :   //}
+     164             : 
+     165             :   /* msgFromTf2() //{ */
+     166      332230 :   static geometry_msgs::Transform msgFromTf2(const tf2::Transform& tf_in) {
+     167             : 
+     168      332230 :     geometry_msgs::Transform tf_out;
+     169      332230 :     tf_out.translation.x = tf_in.getOrigin().getX();
+     170      332230 :     tf_out.translation.y = tf_in.getOrigin().getY();
+     171      332230 :     tf_out.translation.z = tf_in.getOrigin().getZ();
+     172      332230 :     tf_out.rotation = tf2::toMsg(tf_in.getRotation());
+     173             : 
+     174      332230 :     return tf_out;
+     175             :   }
+     176             : 
+     177             :   //}
+     178             :   
+     179             :   /* tf2FromMsg() //{ */
+     180             :   static tf2::Transform tf2FromMsg(const geometry_msgs::Transform& tf_in) {
+     181             : 
+     182             :     tf2::Transform tf_out;
+     183             :     tf_out.setOrigin(tf2::Vector3(tf_in.translation.x, tf_in.translation.y, tf_in.translation.z));
+     184             :     tf_out.setRotation(tf2::Quaternion(tf_in.rotation.x, tf_in.rotation.y, tf_in.rotation.z, tf_in.rotation.w));
+     185             : 
+     186             :     return tf_out;
+     187             :   }
+     188             : 
+     189             :   //}
+     190             :  
+     191             :   /* pointToVector3() //{ */
+     192             : 
+     193      692345 :   static geometry_msgs::Vector3 pointToVector3(const geometry_msgs::Point& point_in) {
+     194             : 
+     195      692345 :     geometry_msgs::Vector3 vec_out;
+     196      692197 :     vec_out.x = point_in.x;
+     197      692197 :     vec_out.y = point_in.y;
+     198      692197 :     vec_out.z = point_in.z;
+     199             : 
+     200      692197 :     return vec_out;
+     201             :   }
+     202             : 
+     203             :   //}
+     204             : 
+     205             :   /*//{ rotateVector() */
+     206      344289 :   static geometry_msgs::Vector3 rotateVector(const geometry_msgs::Vector3& vec_in, const geometry_msgs::Quaternion& q_in) {
+     207             : 
+     208             :     try {
+     209      344289 :       Eigen::Matrix3d        R = mrs_lib::AttitudeConverter(q_in);
+     210      344262 :       Eigen::Vector3d        vec_in_eigen(vec_in.x, vec_in.y, vec_in.z);
+     211      344169 :       Eigen::Vector3d        vec_eigen_rotated = R * vec_in_eigen;
+     212      344182 :       geometry_msgs::Vector3 vec_out;
+     213      343832 :       vec_out.x = vec_eigen_rotated[0];
+     214      343870 :       vec_out.y = vec_eigen_rotated[1];
+     215      343984 :       vec_out.z = vec_eigen_rotated[2];
+     216      344040 :       return vec_out;
+     217             :     }
+     218           0 :     catch (...) {
+     219           0 :       ROS_ERROR_THROTTLE(1.0, "[Support::rotateVector()]: failed");
+     220           0 :       return vec_in;
+     221             :     }
+     222             :   }
+     223             :   /*//}*/
+     224             : 
+     225             :   /*//{ uavStateToOdom() */
+     226      343241 :   static nav_msgs::Odometry uavStateToOdom(const mrs_msgs::UavState& uav_state) {
+     227      343241 :     nav_msgs::Odometry odom;
+     228      343239 :     odom.header              = uav_state.header;
+     229      343231 :     odom.child_frame_id      = uav_state.child_frame_id;
+     230      343224 :     odom.pose.pose           = uav_state.pose;
+     231      343224 :     odom.twist.twist.angular = uav_state.velocity.angular;
+     232             : 
+     233      343224 :     tf2::Quaternion q;
+     234      343225 :     tf2::fromMsg(odom.pose.pose.orientation, q);
+     235      343204 :     odom.twist.twist.linear = Support::rotateVector(uav_state.velocity.linear, mrs_lib::AttitudeConverter(q.inverse()));
+     236             : 
+     237      686132 :     return odom;
+     238             :   }
+     239             :   /*//}*/
+     240             : 
+     241             :   /*//{ getPoseDiff() */
+     242           5 :   static geometry_msgs::Pose getPoseDiff(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2) {
+     243             : 
+     244           5 :     tf2::Vector3 v1, v2;
+     245           5 :     tf2::fromMsg(p1.position, v1);
+     246           5 :     tf2::fromMsg(p2.position, v2);
+     247           5 :     const tf2::Vector3 v3 = v1 - v2;
+     248             : 
+     249           5 :     tf2::Quaternion q1, q2;
+     250           5 :     tf2::fromMsg(p1.orientation, q1);
+     251           5 :     tf2::fromMsg(p2.orientation, q2);
+     252           5 :     tf2::Quaternion q3 = q2 * q1.inverse();
+     253           5 :     q3.normalize();
+     254             : 
+     255           5 :     geometry_msgs::Pose pose_diff;
+     256           5 :     tf2::toMsg(v3, pose_diff.position);
+     257           5 :     pose_diff.orientation = tf2::toMsg(q3);
+     258             : 
+     259          10 :     return pose_diff;
+     260             :   }
+     261             :   /*//}*/
+     262             : 
+     263             :   /*//{ applyPoseDiff() */
+     264      158584 :   static geometry_msgs::Pose applyPoseDiff(const geometry_msgs::Pose& pose_in, const geometry_msgs::Pose& pose_diff) {
+     265             : 
+     266      158584 :     tf2::Vector3    pos_in;
+     267      158584 :     tf2::Quaternion q_in;
+     268      158584 :     tf2::fromMsg(pose_in.position, pos_in);
+     269      158584 :     tf2::fromMsg(pose_in.orientation, q_in);
+     270             : 
+     271      158584 :     tf2::Vector3    pos_diff;
+     272      158584 :     tf2::Quaternion q_diff;
+     273      158584 :     tf2::fromMsg(pose_diff.position, pos_diff);
+     274      158584 :     tf2::fromMsg(pose_diff.orientation, q_diff);
+     275             : 
+     276      158584 :     const tf2::Vector3    pos_out = tf2::quatRotate(q_diff.inverse(), (pos_in - pos_diff));
+     277      158584 :     const tf2::Quaternion q_out   = q_diff.inverse() * q_in;
+     278             : 
+     279      158584 :     geometry_msgs::Pose pose_out;
+     280      158584 :     tf2::toMsg(pos_out, pose_out.position);
+     281      158584 :     pose_out.orientation = tf2::toMsg(q_out);
+     282             : 
+     283      317168 :     return pose_out;
+     284             :   }
+     285             : 
+     286             :   //}
+     287             : 
+     288             :   /*//{ loadParamFile() */
+     289             :   static void loadParamFile(const std::string& file_path, const std::string& ns = "") {
+     290             :     std::string command = "rosparam load " + file_path + " " + ns;
+     291             :     int         result  = std::system(command.c_str());
+     292             :     if (result != 0) {
+     293             :       ROS_ERROR_STREAM("Could not set config file " << file_path << " to the parameter server.");
+     294             :     }
+     295             :   }
+     296             :   /*//}*/
+     297             : 
+     298             :   /*//{ isStringInVector() */
+     299         294 :   static bool isStringInVector(const std::string& value, const std::vector<std::string>& str_vec) {
+     300         294 :     return std::find(str_vec.begin(), str_vec.end(), value) != str_vec.end();
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /*//{ frameIdToEstimatorName() */
+     305          10 :   static std::string frameIdToEstimatorName(const std::string& str_in) {
+     306          20 :     const std::string str_tmp = str_in.substr(str_in.find("/")+1, str_in.size());
+     307          20 :     return str_tmp.substr(0, str_tmp.find("_origin") );
+     308             :   }
+     309             :   /*//}*/
+     310             : 
+     311             : private:
+     312             :   Support() {
+     313             :   }
+     314             : };
+     315             : /*//}*/
+     316             : 
+     317             : }  // namespace estimation_manager
+     318             : 
+     319             : }  // namespace mrs_uav_managers
+     320             : 
+     321             : #endif  // ESTIMATION_MANAGER_SUPPORT_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html new file mode 100644 index 0000000000..4e128092e1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html @@ -0,0 +1,101 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..600621ddc5631c697485f23bb4f61bac03ab2008 GIT binary patch literal 1232 zcmV;>1TXuEP)!aTS z5Vxy9aa$M9sV);`M%x_es-`opvt%zamd49~b&(2;7v*P+*T~+(SeKduCv-i{ynqp} zt`=~5&$R1S-*r3#i7w3&6ffc2>{^8=EOel}ywZVbG_g?7Js zOYN}r?4-J1fl5?sUnT#0=>^cV@jd;UHk^K}S4Vyd4Jhsu~gD~z$WSv=u z(-}ITB2b>r+!Hkg6^)ph9gQKV2U7M^>Y;6?<`EgM0NDh5xxzI|3r{K>X^&q1zIsx? z?q&eEyhkc4iX~{Wz5}I=Nc@!!MDD{sKgmTl0Y<>wzF*tJ58`ZT>%-MJqrFuKM4gpv z`9#;H;BubP5p{v$RVcTfud+wOV|(};Ld}UOBw@S*B#}kG0XQxUNl-ZJnW)eMevv(P z<15cSV0M_Hotv4~0&h&=`+z(HXUIS9&HC9{PB$O}A?e35UiwEgBH*#*5rlozDv;E( z2FKVR-{U)qQs`ND&UP+yDdlJ9;m2k|QdvV0s7cxVZ9rMeo}#<=@fDsTmu{QfRij|_ z6uBOqG$L`f?CtI4e#jP*%jQ5fS`FA5a0l)qW7~2Q5Qn2`dVLG9Z;W>uLWz zf7;CgEfU)E_2TcI1o+SXb{Tt}M&Pg3A5pMuQIj=Obfe)p4Ex8um4U$CaCnR;Jlyt5IPT=mbfITR!t)s$VjotR1Qf{h4W=N?}sNBD{Kl zrB$;+z)x6(on=^Ag=86;aVOAP9CUjDm#wdv}L(8>hN~JiX zO+lPfdvRfM4lya5f;ns~Zn=&_qR-9M!hF>RqWFJA{@Ie9&;jrrA2>9y!lBHRS;F@! zo(XZs4E(pQpZigPK$nIFln5+?CK;ao#J*5WVx2&8)ajD)?Jv6HRJ>?zV& zOtAUFmM=P*2B!e@rv#+7;DsZsSR^>Uhny6g-7DR|)k7)-r}wX?pMR9p{{ShfR7eLX uuF}o73AH3g65Djf>tl2z?7tTjweTN4ektG-6b(NB0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html new file mode 100644 index 0000000000..14aaabaab8 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html new file mode 100644 index 0000000000..58d70f19f5 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html new file mode 100644 index 0000000000..4180b38fe5 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html new file mode 100644 index 0000000000..2770f28cca --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index.html b/mrs_uav_managers/include/mrs_uav_managers/index.html new file mode 100644 index 0000000000..660612f2d1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html new file mode 100644 index 0000000000..b4f63c7ada --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::~StateEstimator()0
mrs_uav_managers::StateEstimator::StateEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)100
mrs_uav_managers::StateEstimator::~StateEstimator().2100
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html new file mode 100644 index 0000000000..94f81f10bc --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::StateEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)100
mrs_uav_managers::StateEstimator::~StateEstimator()0
mrs_uav_managers::StateEstimator::~StateEstimator().2100
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.frameset.html b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.frameset.html new file mode 100644 index 0000000000..62896e21d2 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html new file mode 100644 index 0000000000..81017b6185 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html @@ -0,0 +1,169 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_UAV_MANAGERS_STATE_ESTIMATOR_H
+       2             : #define MRS_UAV_MANAGERS_STATE_ESTIMATOR_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <Eigen/Dense>
+       9             : 
+      10             : #include <nav_msgs/Odometry.h>
+      11             : 
+      12             : #include <mrs_msgs/UavState.h>
+      13             : #include <mrs_msgs/Float64ArrayStamped.h>
+      14             : #include <mrs_msgs/HwApiCapabilities.h>
+      15             : 
+      16             : #include <mrs_uav_managers/estimation_manager/estimator.h>
+      17             : 
+      18             : //}
+      19             : 
+      20             : namespace mrs_uav_managers
+      21             : {
+      22             : 
+      23             : namespace state
+      24             : {
+      25             : const char type[] = "STATE";
+      26             : }
+      27             : 
+      28             : using namespace estimation_manager;
+      29             : 
+      30             : class StateEstimator : public Estimator {
+      31             : 
+      32             : protected:
+      33             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      34             : 
+      35             :   ros::NodeHandle nh_;
+      36             : 
+      37             :   mrs_msgs::UavState uav_state_;
+      38             :   mrs_msgs::UavState uav_state_init_;
+      39             :   mutable std::mutex mtx_uav_state_;
+      40             : 
+      41             :   nav_msgs::Odometry odom_;
+      42             :   mutable std::mutex mtx_odom_;
+      43             : 
+      44             :   nav_msgs::Odometry innovation_;
+      45             :   nav_msgs::Odometry innovation_init_;
+      46             :   mutable std::mutex mtx_innovation_;
+      47             : 
+      48             :   mrs_msgs::Float64ArrayStamped pose_covariance_, twist_covariance_;
+      49             :   mutable std::mutex            mtx_covariance_;
+      50             : 
+      51             :   bool is_override_frame_id_ = false;
+      52             : 
+      53             : protected:
+      54             :   mutable mrs_lib::PublisherHandler<mrs_msgs::UavState>               ph_uav_state_;
+      55             :   mutable mrs_lib::PublisherHandler<nav_msgs::Odometry>               ph_odom_;
+      56             :   mutable mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>    ph_pose_covariance_, ph_twist_covariance_;
+      57             :   mutable mrs_lib::PublisherHandler<nav_msgs::Odometry>               ph_innovation_;
+      58             :   mutable mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped> ph_attitude_;
+      59             : 
+      60             : public:
+      61         100 :   StateEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      62         100 :       : Estimator(state::type, name, frame_id), package_name_(package_name) {
+      63         100 :   }
+      64             : 
+      65         100 :   virtual ~StateEstimator(void) {
+      66         100 :   }
+      67             : 
+      68             :   virtual bool setUavState(const mrs_msgs::UavState &uav_state) = 0;
+      69             : 
+      70             :   // implemented methods
+      71             :   std::optional<mrs_msgs::UavState>        getUavState();
+      72             :   nav_msgs::Odometry                       getInnovation() const;
+      73             :   std::vector<double>                      getPoseCovariance() const;
+      74             :   std::vector<double>                      getTwistCovariance() const;
+      75             :   void                                     publishUavState() const;
+      76             :   void                                     publishOdom() const;
+      77             :   void                                     publishCovariance() const;
+      78             :   void                                     publishInnovation() const;
+      79             :   std::optional<geometry_msgs::Quaternion> rotateQuaternionByHeading(const geometry_msgs::Quaternion &q, const double hdg) const;
+      80             :   bool                                     isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr &hw_api_capabilities) const;
+      81             : };
+      82             : 
+      83             : }  // namespace mrs_uav_managers
+      84             : 
+      85             : #endif  // MRS_UAV_MANAGERS_STATE_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..59d782b49c --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.overview.html @@ -0,0 +1,42 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..023df962b5285e84744bd9112e5b40c2d313589b GIT binary patch literal 435 zcmV;k0ZjghP) zqcGxZtePLT0I_(mFe*!3?98xQ9%(I7vOQF=hDI$Jh%=D-^hj|E5+A__?U8lZohc5w z`*59fKkmyo=lsdwBO?^vJL9|)g^^)5$n*G7f)7&1l}W=3d_nNV!7^&Vay@%91AKfc zuy{sB*{9ZwUjZ_74q{H#s{uNKGm!5FQ$_Ak47O396XE|A&J@#YM(@uA1p+_L>?;PJ d&1tyZ6MnOrXlS<^3&{Wg002ovPDHLkV1nR+yMF)x literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html new file mode 100644 index 0000000000..edb43763a9 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-25 05:42:57Functions:1250.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Tracker::~Tracker()0
mrs_uav_managers::Tracker::~Tracker().2565
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html new file mode 100644 index 0000000000..7695b45c21 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-25 05:42:57Functions:1250.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Tracker::~Tracker()0
mrs_uav_managers::Tracker::~Tracker().2565
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.frameset.html b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.frameset.html new file mode 100644 index 0000000000..e231e20f7c --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html new file mode 100644 index 0000000000..55e6a406f5 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html @@ -0,0 +1,296 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-25 05:42:57Functions:1250.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef MRS_UAV_TRACKER_H
+       2             : #define MRS_UAV_TRACKER_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <mrs_uav_managers/control_manager/common_handlers.h>
+       9             : #include <mrs_uav_managers/control_manager/private_handlers.h>
+      10             : 
+      11             : #include <mrs_msgs/TrackerCommand.h>
+      12             : #include <mrs_msgs/TrackerStatus.h>
+      13             : #include <mrs_msgs/UavState.h>
+      14             : 
+      15             : #include <mrs_msgs/Float64Srv.h>
+      16             : #include <mrs_msgs/Float64SrvRequest.h>
+      17             : #include <mrs_msgs/Float64SrvResponse.h>
+      18             : 
+      19             : #include <mrs_msgs/Float64.h>
+      20             : 
+      21             : #include <mrs_msgs/ReferenceSrv.h>
+      22             : #include <mrs_msgs/ReferenceSrvRequest.h>
+      23             : #include <mrs_msgs/ReferenceSrvResponse.h>
+      24             : 
+      25             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      26             : #include <mrs_msgs/VelocityReferenceSrvRequest.h>
+      27             : #include <mrs_msgs/VelocityReferenceSrvResponse.h>
+      28             : 
+      29             : #include <mrs_msgs/TrajectoryReferenceSrv.h>
+      30             : #include <mrs_msgs/TrajectoryReferenceSrvRequest.h>
+      31             : #include <mrs_msgs/TrajectoryReferenceSrvResponse.h>
+      32             : 
+      33             : #include <std_srvs/Trigger.h>
+      34             : #include <std_srvs/TriggerRequest.h>
+      35             : #include <std_srvs/TriggerResponse.h>
+      36             : 
+      37             : #include <std_srvs/SetBool.h>
+      38             : #include <std_srvs/SetBoolRequest.h>
+      39             : #include <std_srvs/SetBoolResponse.h>
+      40             : 
+      41             : #include <mrs_msgs/DynamicsConstraintsSrv.h>
+      42             : #include <mrs_msgs/DynamicsConstraintsSrvRequest.h>
+      43             : #include <mrs_msgs/DynamicsConstraintsSrvResponse.h>
+      44             : 
+      45             : #include <mrs_uav_managers/controller.h>
+      46             : 
+      47             : //}
+      48             : 
+      49             : namespace mrs_uav_managers
+      50             : {
+      51             : 
+      52             : class Tracker {
+      53             : 
+      54             : public:
+      55             :   /**
+      56             :    * @brief It is called once for every tracker. The runtime is not limited.
+      57             :    *
+      58             :    * @param nh the node handle of the ControlManager
+      59             :    * @param uav_name the UAV name (e.g., "uav1")
+      60             :    * @param common_handlers handlers shared between trackers and controllers
+      61             :    * @param private_handlers handlers provided individually to each tracker
+      62             :    *
+      63             :    * @return true if success
+      64             :    */
+      65             :   virtual bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      66             :                           std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) = 0;
+      67             : 
+      68             :   /**
+      69             :    * @brief It is called before the trackers output will be required and used. Should not take much time (within miliseconds).
+      70             :    *
+      71             :    * @param last_tracker_cmd the last command produced by the last active tracker. Should be used as an initial condition to maintain a smooth reference.
+      72             :    *
+      73             :    * @return true if success and message
+      74             :    */
+      75             :   virtual std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) = 0;
+      76             : 
+      77             :   /**
+      78             :    * @brief is called when this trackers output is no longer needed. However, it can be activated later.
+      79             :    */
+      80             :   virtual void deactivate(void) = 0;
+      81             : 
+      82             :   /**
+      83             :    * @brief It is called during every switch of reference frames of the UAV state estimate.
+      84             :    * The tracker should recalculate its internal states from old the frame to the new one.
+      85             :    *
+      86             :    * @param new_uav_state the new UavState which will come in the next update()
+      87             :    *
+      88             :    * @return a service response
+      89             :    */
+      90             :   virtual const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state) = 0;
+      91             : 
+      92             :   /**
+      93             :    * @brief Request for reseting the tracker's states given the UAV is static.
+      94             :    *
+      95             :    * @return true if success
+      96             :    */
+      97             :   virtual bool resetStatic(void) = 0;
+      98             : 
+      99             :   /**
+     100             :    * @brief The most important routine. It is called with every state estimator update and it should produce a new reference for the controllers.
+     101             :    *        The run time should be as short as possible (<= 1 ms).
+     102             :    *
+     103             :    * @param uav_state the latest UAV state estimate
+     104             :    * @param last_attitude_cmd the last controller's output command (may be useful)
+     105             :    *
+     106             :    * @return the new reference for the controllers
+     107             :    */
+     108             :   virtual std::optional<mrs_msgs::TrackerCommand> update(const mrs_msgs::UavState &uav_state, const Controller::ControlOutput &last_control_output) = 0;
+     109             : 
+     110             :   /**
+     111             :    * @brief A request for the tracker's status.
+     112             :    *
+     113             :    * @return the tracker's status
+     114             :    */
+     115             :   virtual const mrs_msgs::TrackerStatus getStatus() = 0;
+     116             : 
+     117             :   /**
+     118             :    * @brief Request for a flight to a given coordinates.
+     119             :    *
+     120             :    * @param cmd the reference
+     121             :    *
+     122             :    * @return a service response
+     123             :    */
+     124             :   virtual const mrs_msgs::ReferenceSrvResponse::ConstPtr setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) = 0;
+     125             : 
+     126             :   /**
+     127             :    * @brief Request for desired velocity reference
+     128             :    *
+     129             :    * @param cmd the reference
+     130             :    *
+     131             :    * @return a service response
+     132             :    */
+     133             :   virtual const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) = 0;
+     134             : 
+     135             :   /**
+     136             :    * @brief Request for a flight along a given trajectory
+     137             :    *
+     138             :    * @param cmd the reference trajectory
+     139             :    *
+     140             :    * @return a service response
+     141             :    */
+     142             :   virtual const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) = 0;
+     143             : 
+     144             :   /**
+     145             :    * @brief Request for stopping the motion of the UAV.
+     146             :    *
+     147             :    * @param trigger service request
+     148             :    *
+     149             :    * @return a service response
+     150             :    */
+     151             :   virtual const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd) = 0;
+     152             : 
+     153             :   /**
+     154             :    * @brief Request to goto to the first trajectory coordinate.
+     155             :    *
+     156             :    * @param trigger service request
+     157             :    *
+     158             :    * @return a service response
+     159             :    */
+     160             :   virtual const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd) = 0;
+     161             : 
+     162             :   /**
+     163             :    * @brief Request to start tracking of the pre-loaded trajectory
+     164             :    *
+     165             :    * @param trigger service request
+     166             :    *
+     167             :    * @return a service response
+     168             :    */
+     169             :   virtual const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd) = 0;
+     170             : 
+     171             :   /**
+     172             :    * @brief Request to stop tracking of the pre-loaded trajectory. The hover() routine will be engaged, thus it should be implemented by the tracker.
+     173             :    *
+     174             :    * @param trigger service request
+     175             :    *
+     176             :    * @return a service response
+     177             :    */
+     178             :   virtual const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd) = 0;
+     179             : 
+     180             :   /**
+     181             :    * @brief Request to resume the previously stopped trajectory tracking.
+     182             :    *
+     183             :    * @param trigger service request
+     184             :    *
+     185             :    * @return a service response
+     186             :    */
+     187             :   virtual const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd) = 0;
+     188             : 
+     189             :   /**
+     190             :    * @brief Request for enabling/disabling callbacks.
+     191             :    *
+     192             :    * @param cmd service request
+     193             :    *
+     194             :    * @return a service response
+     195             :    */
+     196             :   virtual const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) = 0;
+     197             : 
+     198             :   /**
+     199             :    * @brief Request for setting new constraints.
+     200             :    *
+     201             :    * @param constraints to be set
+     202             :    *
+     203             :    * @return a service response
+     204             :    */
+     205             :   virtual const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) = 0;
+     206             : 
+     207         565 :   virtual ~Tracker() = default;
+     208             : };
+     209             : 
+     210             : }  // namespace mrs_uav_managers
+     211             : 
+     212             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html new file mode 100644 index 0000000000..a0990b0d68 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html @@ -0,0 +1,73 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b561f831d1fbd36bed3927191837491b26db1cec GIT binary patch literal 751 zcmVp{wn9h{fhN)Q+1>Kh+UYP(hJc0`BSsn+CPNKWj|hnq z<0)*3@i`zVJfE>QPz&_v*)wW^qw+LBho)CktARt6Js;21ct&)08r8s&fFAJJZL1l( zz@E+#(66)4XwbM5IRv^zZW$-(*-xzo9?j(YX?%xTALAm%b9Ty_F;@HV40!s?Ay14a z66>?^B)TEnv{sBdR`sb^Mq*_s!eFBB1L(?LuJb11knP}L9V<}uxjuQD*H zkFhcm9{H*|y!052hEop2?ot8@vp;4hJT$n|Q~E&UPpRYy2(p^npi#)s8VZa|R_K6| zOOBv9W?X^^&{i2v8jtN2wZLGmOl35P-`Ou(Mz`sYPwNlwpGJGi?0L!Y&d5b8>50;~ z0BQNI*RV3e9$ch-5C(zUG*S`EyC>DcUP2B-y`gS_o<_P4IG0u{Fjz}iBF-(R^KZL7 zv{J@Ee*>WQ=Wf!T29R5^2x)-Ysmq!vOJfYQFkaJf>oDSPZtP`Q%W-rM)K2~8hS#QR z1%?9&&vyoUvz&%cgx}Zj;<#2|IC!R@$G--somv=&-P^%<!l7lf8xCYN#XB}3 z|3`-N7p=hHG`yS&Ys&?wo%+uhenrA#^%4mp_ZZjfhmYZXjdj9nuO;P~>SOqv(Ya|# zwp#-8DdmOTcQ+g_F`RAh0JT%ggLr!WpF9^@7;k + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19339848.5 %
Date:2024-04-25 05:42:57Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.4%80.4%
+ 		80.4 %193 / 24092.9 %13 / 14
<unnamed>80.4 %193 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html b/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..8b1a4851de --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19339848.5 %
Date:2024-04-25 05:42:57Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.4%80.4%
+ 		80.4 %193 / 24092.9 %13 / 14
<unnamed>80.4 %193 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail.html b/mrs_uav_managers/include/transform_manager/index-detail.html new file mode 100644 index 0000000000..a11b57d0a6 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-detail.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19339848.5 %
Date:2024-04-25 05:42:57Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.4%80.4%
+ 		80.4 %193 / 24092.9 %13 / 14
<unnamed>80.4 %193 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-f.html b/mrs_uav_managers/include/transform_manager/index-sort-f.html new file mode 100644 index 0000000000..a015893641 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19339848.5 %
Date:2024-04-25 05:42:57Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.4%80.4%
+ 		80.4 %193 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-l.html b/mrs_uav_managers/include/transform_manager/index-sort-l.html new file mode 100644 index 0000000000..67dc725c21 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19339848.5 %
Date:2024-04-25 05:42:57Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.4%80.4%
+ 		80.4 %193 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index.html b/mrs_uav_managers/include/transform_manager/index.html new file mode 100644 index 0000000000..d927d0f801 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19339848.5 %
Date:2024-04-25 05:42:57Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.4%80.4%
+ 		80.4 %193 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html new file mode 100644 index 0000000000..5a00f8b36d --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-04-25 05:42:57Functions:050.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfMappingOrigin::getPrintName[abi:cxx11]()0
mrs_uav_managers::TfMappingOrigin::callbackMappingOdomAlt(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::TfMappingOrigin::callbackMappingOdomLat(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::TfMappingOrigin::callbackMappingOdomRot(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_managers::TfMappingOrigin::TfMappingOrigin(ros::NodeHandle, std::shared_ptr<mrs_lib::ParamLoader>, std::shared_ptr<mrs_lib::TransformBroadcaster> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t>)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html new file mode 100644 index 0000000000..bb6e3b11f2 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-04-25 05:42:57Functions:050.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfMappingOrigin::getPrintName[abi:cxx11]()0
mrs_uav_managers::TfMappingOrigin::callbackMappingOdomAlt(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::TfMappingOrigin::callbackMappingOdomLat(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::TfMappingOrigin::callbackMappingOdomRot(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_managers::TfMappingOrigin::TfMappingOrigin(ros::NodeHandle, std::shared_ptr<mrs_lib::ParamLoader>, std::shared_ptr<mrs_lib::TransformBroadcaster> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t>)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.frameset.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.frameset.html new file mode 100644 index 0000000000..ddfde9a414 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html new file mode 100644 index 0000000000..4f512b801e --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html @@ -0,0 +1,474 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-04-25 05:42:57Functions:050.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef TF_MAPPING_ORIGIN_H
+       3             : #define TF_MAPPING_ORIGIN_H
+       4             : 
+       5             : #include <ros/ros.h>
+       6             : 
+       7             : #include <mrs_lib/param_loader.h>
+       8             : #include <mrs_lib/subscribe_handler.h>
+       9             : #include <mrs_lib/transform_broadcaster.h>
+      10             : #include <mrs_lib/attitude_converter.h>
+      11             : #include <mrs_lib/publisher_handler.h>
+      12             : 
+      13             : #include <nav_msgs/Odometry.h>
+      14             : #include <geometry_msgs/QuaternionStamped.h>
+      15             : #include <geometry_msgs/TransformStamped.h>
+      16             : 
+      17             : #include <mrs_uav_managers/estimation_manager/support.h>
+      18             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      19             : 
+      20             : namespace mrs_uav_managers
+      21             : {
+      22             : 
+      23             : /*//{ class TfMappingOrigin */
+      24             : class TfMappingOrigin {
+      25             : 
+      26             :   using Support = estimation_manager::Support;
+      27             : 
+      28             : public:
+      29             :   /*//{ constructor */
+      30           0 :   TfMappingOrigin(ros::NodeHandle nh, std::shared_ptr<mrs_lib::ParamLoader> param_loader, const std::shared_ptr<mrs_lib::TransformBroadcaster>& broadcaster,
+      31             :                   const std::shared_ptr<estimation_manager::CommonHandlers_t> ch)
+      32           0 :       : nh_(nh), broadcaster_(broadcaster), ch_(ch) {
+      33             : 
+      34           0 :     ROS_INFO("[%s]: initializing", getPrintName().c_str());
+      35             : 
+      36           0 :     const std::string yaml_prefix = "mrs_uav_managers/transform_manager/mapping_origin_tf/";
+      37             : 
+      38             :     /*//{ load mapping origin parameters */
+      39           0 :     param_loader->loadParam(yaml_prefix + "debug_prints", debug_prints_);
+      40           0 :     param_loader->loadParam(yaml_prefix + "lateral_topic", lateral_topic_);
+      41           0 :     param_loader->loadParam(yaml_prefix + "altitude_topic", altitude_topic_);
+      42           0 :     param_loader->loadParam(yaml_prefix + "orientation_topic", orientation_topic_);
+      43           0 :     param_loader->loadParam(yaml_prefix + "inverted", tf_inverted_);
+      44           0 :     param_loader->loadParam(yaml_prefix + "custom_frame_id/enabled", custom_frame_id_enabled_);
+      45             : 
+      46           0 :     if (custom_frame_id_enabled_) {
+      47           0 :       param_loader->loadParam(yaml_prefix + "custom_frame_id/frame_id", custom_frame_id_);
+      48             :     }
+      49             : 
+      50           0 :     if (!param_loader->loadedSuccessfully()) {
+      51           0 :       ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      52           0 :       ros::shutdown();
+      53             :     }
+      54             : 
+      55             :     /*//}*/
+      56             : 
+      57             :     /*//{ initialize subscribers */
+      58           0 :     mrs_lib::SubscribeHandlerOptions shopts;
+      59           0 :     shopts.nh                 = nh_;
+      60           0 :     shopts.node_name          = getPrintName();
+      61           0 :     shopts.no_message_timeout = mrs_lib::no_timeout;
+      62           0 :     shopts.threadsafe         = true;
+      63           0 :     shopts.autostart          = true;
+      64           0 :     shopts.queue_size         = 10;
+      65           0 :     shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      66             : 
+      67             :     sh_mapping_odom_lat_ =
+      68           0 :         mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "/" + ch_->uav_name + "/" + lateral_topic_, &TfMappingOrigin::callbackMappingOdomLat, this);
+      69             : 
+      70           0 :     if (orientation_topic_ != lateral_topic_) {
+      71           0 :       sh_mapping_odom_rot_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orientation_topic_.c_str(),
+      72           0 :                                                                                          &TfMappingOrigin::callbackMappingOdomRot, this);
+      73             :     }
+      74             : 
+      75           0 :     if (altitude_topic_ != lateral_topic_) {
+      76           0 :       sh_mapping_odom_alt_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "/" + ch_->uav_name + "/" + altitude_topic_.c_str(),
+      77           0 :                                                                            &TfMappingOrigin::callbackMappingOdomAlt, this);
+      78             :     }
+      79             :     /*//}*/
+      80             : 
+      81           0 :     ph_map_delay_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "map_delay_out", 10);
+      82             : 
+      83           0 :     is_initialized_ = true;
+      84           0 :     ROS_INFO("[%s]: initialized", getPrintName().c_str());
+      85           0 :   }
+      86             :   /*//}*/
+      87             : 
+      88             :   /*//{ getName() */
+      89             :   std::string getName() {
+      90             :     return name_;
+      91             :   }
+      92             :   /*//}*/
+      93             : 
+      94             :   /*//{ getPrintName() */
+      95           0 :   std::string getPrintName() {
+      96           0 :     return ch_->nodelet_name + "/" + name_;
+      97             :   }
+      98             :   /*//}*/
+      99             : 
+     100             : private:
+     101             :   const std::string name_ = "TfMappingOrigin";
+     102             : 
+     103             :   ros::NodeHandle nh_;
+     104             : 
+     105             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     106             : 
+     107             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     108             : 
+     109             :   std::atomic_bool is_initialized_ = false;
+     110             : 
+     111             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_map_delay_;
+     112             : 
+     113             :   bool        debug_prints_;
+     114             :   bool        tf_inverted_;
+     115             :   bool        custom_frame_id_enabled_;
+     116             :   std::string custom_frame_id_;
+     117             :   double      cache_duration_;
+     118             : 
+     119             :   std::string                                   lateral_topic_;
+     120             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_mapping_odom_lat_;
+     121             :   std::vector<nav_msgs::Odometry>               vec_mapping_odom_lat_;
+     122             :   std::atomic_bool                              got_mapping_odom_lat_ = false;
+     123             :   std::atomic<double>                           timestamp_lat_;
+     124             : 
+     125             :   /*//{ callbackMappingOdomLat() */
+     126             : 
+     127           0 :   void callbackMappingOdomLat(const nav_msgs::Odometry::ConstPtr msg) {
+     128             : 
+     129           0 :     if (!is_initialized_) {
+     130           0 :       return;
+     131             :     }
+     132             : 
+     133           0 :     timestamp_lat_ = msg->header.stamp.toSec();
+     134             : 
+     135           0 :     if (!got_mapping_odom_lat_) {
+     136           0 :       got_mapping_odom_lat_ = true;
+     137             :     }
+     138             : 
+     139           0 :     if (!got_mapping_odom_rot_ && orientation_topic_ == lateral_topic_) {
+     140           0 :       got_mapping_odom_rot_ = true;
+     141             :     }
+     142             : 
+     143           0 :     if (!got_mapping_odom_alt_ && altitude_topic_ == lateral_topic_) {
+     144           0 :       got_mapping_odom_alt_ = true;
+     145             :     }
+     146             : 
+     147           0 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TfMappingOrigin::callbackMappingOdomLat", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     148             : 
+     149           0 :     const double hdg_mapping_old = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading();
+     150             : 
+     151             :     /* publish mapping origin tf //{ */
+     152             : 
+     153           0 :     bool clear_needed = false;
+     154             : 
+     155           0 :     if (got_mapping_odom_rot_ && got_mapping_odom_alt_) {
+     156           0 :       std::scoped_lock lock(mtx_mapping_odom_rot_);
+     157             : 
+     158             :       // Copy mapping odometry
+     159           0 :       nav_msgs::Odometry mapping_odom;
+     160           0 :       mapping_odom = *msg;
+     161             : 
+     162             :       // Find corresponding orientation
+     163           0 :       geometry_msgs::QuaternionStamped rot_tmp           = *sh_mapping_odom_rot_.getMsg();  // start with newest msg
+     164           0 :       ros::Time                        dbg_timestamp_rot = rot_tmp.header.stamp;
+     165           0 :       tf2::Quaternion                  tf2_rot;
+     166             : 
+     167           0 :       for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) {
+     168           0 :         if (mapping_odom.header.stamp < vec_mapping_odom_rot_.at(i).header.stamp) {
+     169             : 
+     170             :           // Choose an orientation with closest timestamp
+     171           0 :           double time_diff      = std::fabs(vec_mapping_odom_rot_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     172           0 :           double time_diff_prev = std::numeric_limits<double>::max();
+     173           0 :           if (i > 0) {
+     174           0 :             time_diff_prev = std::fabs(vec_mapping_odom_rot_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     175             :           }
+     176           0 :           if (time_diff_prev < time_diff && i > 0) {
+     177           0 :             i = i - 1;
+     178             :           }
+     179             : 
+     180             :           // Cache is too small if it is full and its oldest element is used
+     181           0 :           if (clear_needed && i == 0) {
+     182           0 :             ROS_WARN_THROTTLE(1.0, "[%s] Mapping orientation cache is too small.", getPrintName().c_str());
+     183             :           }
+     184           0 :           rot_tmp           = vec_mapping_odom_rot_.at(i);
+     185           0 :           dbg_timestamp_rot = vec_mapping_odom_rot_.at(i).header.stamp;
+     186           0 :           break;
+     187             :         }
+     188             :       }
+     189             : 
+     190           0 :       tf2_rot = mrs_lib::AttitudeConverter(rot_tmp.quaternion);
+     191             : 
+     192             :       // Obtain heading from orientation
+     193           0 :       double hdg = 0;
+     194             :       try {
+     195           0 :         hdg = mrs_lib::AttitudeConverter(rot_tmp.quaternion).getHeading();
+     196             :       }
+     197           0 :       catch (...) {
+     198           0 :         ROS_WARN("[%s]: failed to getHeading() from rot_tmp", getPrintName().c_str());
+     199             :       }
+     200             : 
+     201             :       // Build rotation matrix from difference between new heading and old heading
+     202           0 :       tf2::Matrix3x3 rot_mat = mrs_lib::AttitudeConverter(Eigen::AngleAxisd(hdg_mapping_old - hdg, Eigen::Vector3d::UnitZ()));
+     203             : 
+     204             :       // Transform the mavros orientation by the rotation matrix
+     205           0 :       geometry_msgs::Quaternion new_orientation = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_rot);
+     206             : 
+     207             :       // Set new orientation
+     208           0 :       mapping_odom.pose.pose.orientation = new_orientation;
+     209             : 
+     210             : 
+     211             :       // Find corresponding local odom
+     212           0 :       double    odom_alt          = msg->pose.pose.position.z;  // start with newest msg
+     213           0 :       ros::Time dbg_timestamp_alt = msg->header.stamp;
+     214           0 :       for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) {
+     215           0 :         if (mapping_odom.header.stamp < vec_mapping_odom_alt_.at(i).header.stamp) {
+     216             : 
+     217             :           // Choose orientation with closest timestamp
+     218           0 :           double time_diff      = std::fabs(vec_mapping_odom_alt_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     219           0 :           double time_diff_prev = std::numeric_limits<double>::max();
+     220           0 :           if (i > 0) {
+     221           0 :             time_diff_prev = std::fabs(vec_mapping_odom_alt_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec());
+     222             :           }
+     223           0 :           if (time_diff_prev < time_diff && i > 0) {
+     224           0 :             i = i - 1;
+     225             :           }
+     226             :           // Cache is too small if it is full and its oldest element is used
+     227           0 :           if (clear_needed && i == 0) {
+     228           0 :             ROS_WARN_THROTTLE(1.0, "[%s] mapping orientation cache (for mapping tf) is too small.", getPrintName().c_str());
+     229             :           }
+     230           0 :           odom_alt          = vec_mapping_odom_alt_.at(i).pose.pose.position.z;
+     231           0 :           dbg_timestamp_alt = vec_mapping_odom_alt_.at(i).header.stamp;
+     232           0 :           break;
+     233             :         }
+     234             :       }
+     235             : 
+     236             :       // Set altitude
+     237           0 :       mapping_odom.pose.pose.position.z = odom_alt;
+     238             : 
+     239             :       // Get inverse transform
+     240           0 :       tf2::Transform      tf_mapping_inv   = Support::tf2FromPose(mapping_odom.pose.pose).inverse();
+     241           0 :       geometry_msgs::Pose pose_mapping_inv = Support::poseFromTf2(tf_mapping_inv);
+     242             : 
+     243           0 :       geometry_msgs::TransformStamped tf_mapping;
+     244           0 :       tf_mapping.header.stamp    = mapping_odom.header.stamp;
+     245           0 :       tf_mapping.header.frame_id = ch_->frames.ns_fcu;
+     246           0 :       if (custom_frame_id_enabled_) {
+     247           0 :         tf_mapping.child_frame_id = ch_->uav_name + "/" + custom_frame_id_;
+     248             :       } else {
+     249           0 :         tf_mapping.child_frame_id = mapping_odom.header.frame_id;
+     250             :       }
+     251           0 :       tf_mapping.transform.translation = Support::pointToVector3(pose_mapping_inv.position);
+     252           0 :       tf_mapping.transform.rotation    = pose_mapping_inv.orientation;
+     253             : 
+     254           0 :       if (Support::noNans(tf_mapping)) {
+     255             :         try {
+     256           0 :           broadcaster_->sendTransform(tf_mapping);
+     257             :         }
+     258           0 :         catch (...) {
+     259           0 :           ROS_ERROR("[%s]: Exception caught during publishing TF: %s - %s.", getPrintName().c_str(), tf_mapping.child_frame_id.c_str(),
+     260             :                     tf_mapping.header.frame_id.c_str());
+     261             :         }
+     262             :       } else {
+     263           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_mapping.header.frame_id.c_str(),
+     264             :                           tf_mapping.child_frame_id.c_str());
+     265             :       }
+     266             : 
+     267             :       // debug timestamps: rot and alt timestamps should be as close as possible to lat_timestamp, the delay is the difference between current time and the time
+     268             :       // of acquisition of scan that was used to calculate the pose estimate
+     269           0 :       if (debug_prints_) {
+     270           0 :         ROS_INFO("[%s] lat timestamp: %.6f, rot stamp: %.6f (diff: %.6f),  alt stamp: %.6f (diff: %.6f), delay: %.6f", getPrintName().c_str(),
+     271             :                  mapping_odom.header.stamp.toSec(), dbg_timestamp_rot.toSec(), dbg_timestamp_rot.toSec() - mapping_odom.header.stamp.toSec(),
+     272             :                  dbg_timestamp_alt.toSec(), dbg_timestamp_alt.toSec() - mapping_odom.header.stamp.toSec(),
+     273             :                  ros::Time::now().toSec() - mapping_odom.header.stamp.toSec());
+     274             :       }
+     275             : 
+     276           0 :       mrs_msgs::Float64Stamped map_delay_msg;
+     277           0 :       map_delay_msg.header.stamp = ros::Time::now();
+     278           0 :       map_delay_msg.value        = ros::Time::now().toSec() - mapping_odom.header.stamp.toSec();
+     279           0 :       ph_map_delay_.publish(map_delay_msg);
+     280             :     }
+     281             : 
+     282             :     //}
+     283             :   }
+     284             :   /*//}*/
+     285             : 
+     286             :   std::string                                                 orientation_topic_;
+     287             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_mapping_odom_rot_;
+     288             :   std::vector<geometry_msgs::QuaternionStamped>               vec_mapping_odom_rot_;
+     289             :   std::mutex                                                  mtx_mapping_odom_rot_;
+     290             :   std::atomic_bool                                            got_mapping_odom_rot_ = false;
+     291             : 
+     292             :   /*//{ callbackMappingOdomRot() */
+     293           0 :   void callbackMappingOdomRot(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     294             : 
+     295           0 :     if (!is_initialized_) {
+     296           0 :       return;
+     297             :     }
+     298             : 
+     299           0 :     if (!got_mapping_odom_lat_) {
+     300           0 :       return;
+     301             :     }
+     302             : 
+     303           0 :     std::scoped_lock lock(mtx_mapping_odom_rot_);
+     304             : 
+     305             :     // Add new data
+     306           0 :     vec_mapping_odom_rot_.push_back(*msg);
+     307             : 
+     308             :     // Delete old data
+     309           0 :     size_t index_delete = 0;
+     310           0 :     bool   clear_needed = false;
+     311           0 :     for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) {
+     312           0 :       if (timestamp_lat_ - vec_mapping_odom_rot_.at(i).header.stamp.toSec() > cache_duration_) {
+     313           0 :         index_delete = i;
+     314           0 :         clear_needed = true;
+     315             :       } else {
+     316           0 :         break;
+     317             :       }
+     318             :     }
+     319           0 :     if (clear_needed) {
+     320           0 :       for (int i = (int)index_delete; i >= 0; i--) {
+     321           0 :         vec_mapping_odom_rot_.erase(vec_mapping_odom_rot_.begin() + i);
+     322             :       }
+     323           0 :       clear_needed = false;
+     324             :     }
+     325             : 
+     326           0 :     if (!got_mapping_odom_rot_) {
+     327           0 :       got_mapping_odom_rot_ = true;
+     328             :     }
+     329             : 
+     330           0 :     if (debug_prints_) {
+     331           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: mapping odom rot cache size: %lu", getPrintName().c_str(), vec_mapping_odom_rot_.size());
+     332             :     }
+     333             :   }
+     334             :   /*//}*/
+     335             : 
+     336             :   std::string                                   altitude_topic_;
+     337             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_mapping_odom_alt_;
+     338             :   std::vector<nav_msgs::Odometry>               vec_mapping_odom_alt_;
+     339             :   std::mutex                                    mtx_mapping_odom_alt_;
+     340             :   std::atomic_bool                              got_mapping_odom_alt_ = false;
+     341             : 
+     342             :   /*//{ callbackMappingOdomAlt() */
+     343           0 :   void callbackMappingOdomAlt(const nav_msgs::Odometry::ConstPtr msg) {
+     344             : 
+     345           0 :     if (!is_initialized_) {
+     346           0 :       return;
+     347             :     }
+     348             : 
+     349           0 :     if (!got_mapping_odom_lat_) {
+     350           0 :       return;
+     351             :     }
+     352             : 
+     353           0 :     std::scoped_lock lock(mtx_mapping_odom_alt_);
+     354             : 
+     355             :     // Add new data
+     356           0 :     vec_mapping_odom_alt_.push_back(*msg);
+     357             : 
+     358             :     // Delete old data
+     359           0 :     size_t index_delete = 0;
+     360           0 :     bool   clear_needed = false;
+     361           0 :     for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) {
+     362           0 :       if (timestamp_lat_ - vec_mapping_odom_alt_.at(i).header.stamp.toSec() > cache_duration_) {
+     363           0 :         index_delete = i;
+     364           0 :         clear_needed = true;
+     365             :       } else {
+     366           0 :         break;
+     367             :       }
+     368             :     }
+     369           0 :     if (clear_needed) {
+     370           0 :       for (int i = (int)index_delete; i >= 0; i--) {
+     371           0 :         vec_mapping_odom_alt_.erase(vec_mapping_odom_alt_.begin() + i);
+     372             :       }
+     373           0 :       clear_needed = false;
+     374             :     }
+     375             : 
+     376           0 :     if (!got_mapping_odom_alt_) {
+     377           0 :       got_mapping_odom_alt_ = true;
+     378             :     }
+     379             : 
+     380           0 :     if (debug_prints_) {
+     381           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: mapping odom alt cache size: %lu", getPrintName().c_str(), vec_mapping_odom_alt_.size());
+     382             :     }
+     383             :   }
+     384             :   /*//}*/
+     385             : };
+     386             : /*//}*/
+     387             : 
+     388             : }  // namespace mrs_uav_managers
+     389             : 
+     390             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html new file mode 100644 index 0000000000..8218f6ae36 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html @@ -0,0 +1,118 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e3d425e3e4319475911ce94a36b8a1ec26ea482f GIT binary patch literal 1536 zcmV+b2LJhqP)fFt&%}Y=;1N(Rt_>#|c)`|cl;5Az%*)9Km%cd5r zUFGeDo-=U20yZ*9J;hlZ%g6PN%&PDs(; zZQc_ql5}Li1&Wc7j2)Hi6v5(q4xMcKUGtTf7AM^CH`7`Hq6Oj%SYJ$l#Hs^eflJTm z6p`qXXZXg6UEq{=1lAoAtqYAy$_SbcWen$csNogxCj?gk&dhVSGAl{NP7wm29|q@Y zJs4C201!2gJu7X(j5MBlK)G+azz7s|2PmwkK%m&);SVZSo1!8DZ7cg4);@CJiEei| z@#X0H03yC>iXcB^isGE>I+Nt-zrYjN%B+1Rv4N zlHIWnIcxoG_QBZN1S=8k+u|La^#LJA-*Fo7%06O(&vKt-d2wQvr|baD(zTYWcZ@0& zP1W!{os`xbscoT+wK$x073aO#paFUR0W!q)WsE;mM%nOwSVkT#oz`@HFoZgGdiJF= z7Yv2+U}%3iS9nYnN!^h$cCCq?k^K=lkPbA+iVa1xc~1MdQV2#`S7!t7uGZCeW388E z#CoN>(6^^%oM_OURDjfKwBv1n6dPJ|uld@;H1_FdxxGX6cb)-=S)A0a$Y{1>js{mI zL)Gz4*s-1hyf*rvtm}qQ1dPNy&mvYQQ@^#7Xl5^5uId|p8ohg&dsTWvo$Go-Dzv0|7P7xJ!4NNnyJ3y7(SM_ z^0KpDSt+z2)$L?BzT?m5Z<6d&q;$uH%f1FiWfHNYEW+1yTb19^SF5b|`O^iyEt*yL zOSgNAo~ebsJDl*4Ct!cd^y71Oi?y($AuEu}=zl~RK57!!TQ?>6<6DMD6wPJ$KUdeh zxz?+zw!Nv{|5kO~AW2PUK)y{&|FoPEg2)(oe_G9(Z$1*qEu(GOW{kc?k!N763n#bLRnqN0) zx1>EhgkziLaw*D{3X&4MJaWlONLPEx0`s)C&V3E00k?sLLUCoJ=4s+YJYR$wu_qSU zaNhCJ(%yW;mV1N2g@;R@q?(7Thw2Z2Cmoh1B0tYGn@hh=eaE9kMmv7={(3|ccg5&V zlkK;X z!aARa46O0;V?gl)dc!TrW;}=UPO3YbiFpQqHxMJQGvMf)wA{LyfYvFOG2d3}jJvop z`5?wl2i-=$rVm$ajVH~;KD_ft5dnTUrBoF^bLBhSYoe!s9y~%vhu=$2EB?F=pslL2 z(tq&*mX1U%u*DJ{PNg)``koHp&pwy(?EoyS$ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19324080.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::TfSource::setIsUtmSource(bool)10
mrs_uav_managers::TfSource::getIsUtmSource()18
mrs_uav_managers::TfSource::getIsUtmBased()20
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)100
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)100
mrs_uav_managers::TfSource::TfSource(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle, std::shared_ptr<mrs_lib::ParamLoader>, std::shared_ptr<mrs_lib::TransformBroadcaster> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t>, bool)100
mrs_uav_managers::TfSource::getName[abi:cxx11]()819
mrs_uav_managers::TfSource::republishInFrame(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >&)139339
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)184496
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)184543
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)190527
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)190552
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()886278
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.func.html b/mrs_uav_managers/include/transform_manager/tf_source.h.func.html new file mode 100644 index 0000000000..912503d8ee --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.func.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19324080.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()886278
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)100
mrs_uav_managers::TfSource::getIsUtmBased()20
mrs_uav_managers::TfSource::getIsUtmSource()18
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::TfSource::setIsUtmSource(bool)10
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)100
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)190527
mrs_uav_managers::TfSource::republishInFrame(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >&)139339
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)184496
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)190552
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)184543
mrs_uav_managers::TfSource::getName[abi:cxx11]()819
mrs_uav_managers::TfSource::TfSource(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle, std::shared_ptr<mrs_lib::ParamLoader>, std::shared_ptr<mrs_lib::TransformBroadcaster> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t>, bool)100
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.frameset.html b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.frameset.html new file mode 100644 index 0000000000..4f63eb0600 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html new file mode 100644 index 0000000000..be756bf13a --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html @@ -0,0 +1,704 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19324080.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef TF_SOURCE_H
+       3             : #define TF_SOURCE_H
+       4             : 
+       5             : #include <ros/ros.h>
+       6             : 
+       7             : #include <mrs_lib/param_loader.h>
+       8             : #include <mrs_lib/subscribe_handler.h>
+       9             : #include <mrs_lib/publisher_handler.h>
+      10             : #include <mrs_lib/attitude_converter.h>
+      11             : #include <mrs_lib/transformer.h>
+      12             : #include <mrs_lib/transform_broadcaster.h>
+      13             : #include <mrs_lib/gps_conversions.h>
+      14             : 
+      15             : #include <tf2_ros/transform_broadcaster.h>
+      16             : #include <tf2_ros/static_transform_broadcaster.h>
+      17             : 
+      18             : #include <geometry_msgs/TransformStamped.h>
+      19             : 
+      20             : #include <nav_msgs/Odometry.h>
+      21             : 
+      22             : #include <memory>
+      23             : #include <string>
+      24             : 
+      25             : #include <mrs_uav_managers/estimation_manager/support.h>
+      26             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      27             : 
+      28             : namespace mrs_uav_managers
+      29             : {
+      30             : 
+      31             : /*//{ class TfSource */
+      32             : class TfSource {
+      33             : 
+      34             :   using Support = estimation_manager::Support;
+      35             : 
+      36             : public:
+      37             :   /*//{ constructor */
+      38         100 :   TfSource(const std::string& name, ros::NodeHandle nh, std::shared_ptr<mrs_lib::ParamLoader> param_loader,
+      39             :            const std::shared_ptr<mrs_lib::TransformBroadcaster>& broadcaster, const std::shared_ptr<estimation_manager::CommonHandlers_t> ch,
+      40             :            const bool is_utm_source)
+      41         100 :       : name_(name), nh_(nh), broadcaster_(broadcaster), ch_(ch), is_utm_source_(is_utm_source) {
+      42             : 
+      43         100 :     ROS_INFO("[%s]: initializing", getPrintName().c_str());
+      44             : 
+      45         100 :     if (name != "dummy") {
+      46             : 
+      47             :       /*//{ load parameters */
+      48             : 
+      49         200 :       const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+      50             : 
+      51         200 :       std::string odom_topic, attitude_topic, ns;
+      52             : 
+      53         100 :       param_loader->loadParam(yaml_prefix + getName() + "/odom_topic", odom_topic);
+      54         100 :       param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/enabled", custom_frame_id_enabled_, false);
+      55         100 :       if (custom_frame_id_enabled_) {
+      56           0 :         param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/frame_id", custom_frame_id_);
+      57             :       }
+      58         100 :       param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/enabled", tf_from_attitude_enabled_);
+      59         100 :       if (tf_from_attitude_enabled_) {
+      60          99 :         param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/attitude_topic", attitude_topic);
+      61             :       }
+      62         100 :       param_loader->loadParam(yaml_prefix + getName() + "/namespace", ns);
+      63         100 :       full_topic_odom_     = "/" + ch_->uav_name + "/" + ns + "/" + odom_topic;
+      64         100 :       full_topic_attitude_ = "/" + ch_->uav_name + "/" + ns + "/" + attitude_topic;
+      65         100 :       param_loader->loadParam(yaml_prefix + getName() + "/inverted", is_inverted_);
+      66         100 :       param_loader->loadParam(yaml_prefix + getName() + "/republish_in_frames", republish_in_frames_);
+      67             : 
+      68             :       /* coordinate frames origins //{ */
+      69         100 :       param_loader->loadParam(yaml_prefix + getName() + "/utm_based", is_utm_based_);
+      70             :       /* param_loader->loadParam(yaml_prefix + getName() + "/publish_local_tf", publish_local_tf_); */
+      71             : 
+      72             :       /*//{ utm source */
+      73         100 :       if (is_utm_based_) {
+      74         198 :         std::string utm_origin_parent_frame_id;
+      75          99 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/parent", utm_origin_parent_frame_id);
+      76          99 :         ns_utm_origin_parent_frame_id_ = ch_->uav_name + "/" + utm_origin_parent_frame_id;
+      77             : 
+      78          99 :         std::string utm_origin_child_frame_id;
+      79          99 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/child", utm_origin_child_frame_id);
+      80          99 :         ns_utm_origin_child_frame_id_ = ch_->uav_name + "/" + utm_origin_child_frame_id;
+      81             :       }
+      82             :       /*//}*/
+      83             : 
+      84             :       /*//{ world source */
+      85         100 :       if (is_utm_based_) {
+      86         198 :         std::string world_origin_parent_frame_id;
+      87          99 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/parent", world_origin_parent_frame_id);
+      88          99 :         ns_world_origin_parent_frame_id_ = ch_->uav_name + "/" + world_origin_parent_frame_id;
+      89             : 
+      90          99 :         std::string world_origin_child_frame_id;
+      91          99 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/child", world_origin_child_frame_id);
+      92          99 :         ns_world_origin_child_frame_id_ = ch_->uav_name + "/" + world_origin_child_frame_id;
+      93             :       }
+      94             :       /*//}*/
+      95             : 
+      96             :       //}
+      97             : 
+      98         100 :       if (!param_loader->loadedSuccessfully()) {
+      99           0 :         ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     100           0 :         ros::shutdown();
+     101             :       }
+     102             : 
+     103             :       /*//}*/
+     104             : 
+     105             :       /*//{ initialize subscribers */
+     106         200 :       mrs_lib::SubscribeHandlerOptions shopts;
+     107         100 :       shopts.nh                 = nh_;
+     108         100 :       shopts.node_name          = getPrintName();
+     109         100 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     110         100 :       shopts.threadsafe         = true;
+     111         100 :       shopts.autostart          = true;
+     112         100 :       shopts.queue_size         = 10;
+     113         100 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     114             : 
+     115         100 :       sh_tf_source_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, full_topic_odom_, &TfSource::callbackTfSourceOdom, this);
+     116         100 :       if (tf_from_attitude_enabled_) {
+     117          99 :         sh_tf_source_att_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, full_topic_attitude_, &TfSource::callbackTfSourceAtt, this);
+     118             :       }
+     119             : 
+     120         100 :       if (is_utm_source_) {
+     121          93 :         sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     122             :       }
+     123             : 
+     124             :     }
+     125             :     /*//}*/
+     126             : 
+     127         176 :     for (auto frame_id : republish_in_frames_) {
+     128          76 :       republishers_.push_back(
+     129         152 :           std::make_pair(frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, full_topic_odom_ + "/" + frame_id.substr(0, frame_id.find("_origin")))));
+     130             :     }
+     131         100 :     is_initialized_ = true;
+     132         100 :     ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     133         100 :   }
+     134             :   /*//}*/
+     135             : 
+     136             :   /*//{ getName() */
+     137         819 :   std::string getName() {
+     138         819 :     return name_;
+     139             :   }
+     140             :   /*//}*/
+     141             : 
+     142             :   /*//{ getPrintName() */
+     143      886278 :   std::string getPrintName() {
+     144     1774914 :     return ch_->nodelet_name + "/" + name_;
+     145             :   }
+     146             :   /*//}*/
+     147             : 
+     148             :   /*//{ getIsUtmBased() */
+     149          20 :   bool getIsUtmBased() {
+     150          20 :     return is_utm_based_;
+     151             :   }
+     152             :   /*//}*/
+     153             : 
+     154             :   /*//{ getIsUtmSource() */
+     155          18 :   bool getIsUtmSource() {
+     156          18 :     return is_utm_source_;
+     157             :   }
+     158             :   /*//}*/
+     159             : 
+     160             :   /*//{ setIsUtmSource() */
+     161          10 :   void setIsUtmSource(const bool is_utm_source) {
+     162          10 :     if (is_utm_source) {
+     163           5 :       mrs_lib::SubscribeHandlerOptions shopts;
+     164           5 :       shopts.nh                 = nh_;
+     165           5 :       shopts.node_name          = getPrintName();
+     166           5 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     167           5 :       shopts.threadsafe         = true;
+     168           5 :       shopts.autostart          = true;
+     169           5 :       shopts.queue_size         = 10;
+     170           5 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     171           5 :       sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     172             :     }
+     173          10 :     is_utm_source_ = is_utm_source;
+     174          10 :   }
+     175             :   /*//}*/
+     176             : 
+     177             :   /*//{ setUtmOrigin() */
+     178         100 :   void setUtmOrigin(const geometry_msgs::Point& pt) {
+     179             : 
+     180         100 :     if (is_utm_based_ && !is_utm_origin_set_) {
+     181          99 :       utm_origin_        = pt;
+     182          99 :       is_utm_origin_set_ = true;
+     183             :     }
+     184         100 :   }
+     185             :   /*//}*/
+     186             : 
+     187             :   /*//{ setWorldOrigin() */
+     188         100 :   void setWorldOrigin(const geometry_msgs::Point& pt) {
+     189             : 
+     190         100 :     if (is_utm_based_ && !is_world_origin_set_) {
+     191          99 :       world_origin_        = pt;
+     192          99 :       is_world_origin_set_ = true;
+     193             :     }
+     194         100 :   }
+     195             :   /*//}*/
+     196             : 
+     197             : private:
+     198             :   const std::string name_;
+     199             :   const std::string ns_fcu_frame_id_;
+     200             : 
+     201             :   ros::NodeHandle nh_;
+     202             : 
+     203             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     204             :   tf2_ros::StaticTransformBroadcaster            static_broadcaster_;
+     205             : 
+     206             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     207             : 
+     208             :   bool is_inverted_;
+     209             : 
+     210             :   bool             is_utm_based_;
+     211             :   bool             publish_local_tf_ = false;
+     212             :   bool             publish_utm_tf_   = false;
+     213             :   bool             publish_world_tf_ = false;
+     214             :   std::atomic_bool is_utm_source_    = false;
+     215             :   std::string      ns_utm_origin_parent_frame_id_;
+     216             :   std::string      ns_utm_origin_child_frame_id_;
+     217             : 
+     218             :   bool                 is_utm_origin_set_ = false;
+     219             :   geometry_msgs::Point utm_origin_;
+     220             : 
+     221             :   std::string ns_world_origin_parent_frame_id_;
+     222             :   std::string ns_world_origin_child_frame_id_;
+     223             : 
+     224             :   bool                 is_world_origin_set_ = false;
+     225             :   geometry_msgs::Point world_origin_;
+     226             : 
+     227             :   std::string full_topic_odom_;
+     228             :   std::string full_topic_attitude_;
+     229             :   bool        tf_from_attitude_enabled_ = false;
+     230             : 
+     231             :   bool        custom_frame_id_enabled_;
+     232             :   std::string custom_frame_id_;
+     233             : 
+     234             :   std::atomic_bool is_initialized_               = false;
+     235             :   std::atomic_bool is_local_static_tf_published_ = false;
+     236             :   std::atomic_bool is_utm_static_tf_published_   = false;
+     237             :   std::atomic_bool is_world_static_tf_published_ = false;
+     238             : 
+     239             :   std::vector<std::string> republish_in_frames_;
+     240             : 
+     241             :   std::vector<std::pair<std::string, mrs_lib::PublisherHandler<nav_msgs::Odometry>>> republishers_;
+     242             : 
+     243             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry>               sh_tf_source_odom_;
+     244             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_tf_source_att_;
+     245             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>          sh_altitude_amsl_;
+     246             :   nav_msgs::OdometryConstPtr                                  first_msg_;
+     247             :   bool                                                        got_first_msg_ = false;
+     248             : 
+     249             : 
+     250             :   /*//{ callbackTfSourceOdom()*/
+     251      184543 :   void callbackTfSourceOdom(const nav_msgs::Odometry::ConstPtr msg) {
+     252             : 
+     253      184543 :     if (!is_initialized_) {
+     254           0 :       return;
+     255             :     }
+     256             : 
+     257      553579 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     258             : 
+     259      184507 :     scope_timer.checkpoint("get msg");
+     260             : 
+     261      184547 :     if (!got_first_msg_) {
+     262         100 :       first_msg_     = msg;
+     263         100 :       got_first_msg_ = true;
+     264             :     }
+     265             : 
+     266      184547 :     publishTfFromOdom(msg);
+     267      184562 :     scope_timer.checkpoint("pub tf");
+     268             : 
+     269      184550 :     if (publish_local_tf_ && !is_local_static_tf_published_) {
+     270           0 :       publishLocalTf(msg->header.frame_id);
+     271           0 :       scope_timer.checkpoint("pub local tf");
+     272             :     }
+     273             : 
+     274             :     /* if (publish_utm_tf_ && is_utm_based_ && is_utm_origin_set_ && !is_utm_static_tf_published_) { */
+     275             :     /*   publishUtmTf(msg->header.frame_id); */
+     276             :     /*   scope_timer.checkpoint("pub utm tf"); */
+     277             :     /* } */
+     278             : 
+     279             :     /* if (publish_world_tf_ && is_utm_based_ && is_world_origin_set_ && !is_world_static_tf_published_) { */
+     280             :     /*   publishWorldTf(msg->header.frame_id); */
+     281             :     /*   scope_timer.checkpoint("pub world tf"); */
+     282             :     /* } */
+     283             : 
+     284      323889 :     for (auto republisher : republishers_) {
+     285      139339 :       republishInFrame(msg, ch_->uav_name + "/" + republisher.first, republisher.second);
+     286             :     }
+     287             :   }
+     288             :   /*//}*/
+     289             : 
+     290             :   /*//{ callbackTfSourceAtt()*/
+     291      190552 :   void callbackTfSourceAtt(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     292             : 
+     293      190552 :     if (!is_initialized_) {
+     294           0 :       return;
+     295             :     }
+     296             : 
+     297      571630 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299      190524 :     scope_timer.checkpoint("get msg");
+     300      190545 :     publishTfFromAtt(msg);
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /* publishTfFromOdom() //{*/
+     305      184496 :   void publishTfFromOdom(const nav_msgs::OdometryConstPtr& odom) {
+     306             : 
+     307      369048 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     308             : 
+     309      184529 :     const tf2::Transform      tf       = Support::tf2FromPose(odom->pose.pose);
+     310      184516 :     const tf2::Transform      tf_inv   = tf.inverse();
+     311      184509 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     312             : 
+     313             :     /*//{ tf source origin */
+     314      184528 :     geometry_msgs::TransformStamped tf_msg;
+     315      184469 :     tf_msg.header.stamp         = odom->header.stamp;
+     316      184492 :     std::string origin_frame_id = custom_frame_id_enabled_ ? ch_->uav_name + "/" + custom_frame_id_ : odom->header.frame_id;
+     317      184518 :     if (is_inverted_) {
+     318             : 
+     319      184518 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     320      184519 :       tf_msg.child_frame_id        = origin_frame_id;
+     321      184474 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     322      184540 :       tf_msg.transform.rotation    = pose_inv.orientation;
+     323             : 
+     324             :     } else {
+     325           0 :       tf_msg.header.frame_id       = origin_frame_id;
+     326           0 :       tf_msg.child_frame_id        = ch_->frames.ns_fcu;
+     327           0 :       tf_msg.transform.translation = Support::pointToVector3(odom->pose.pose.position);
+     328           0 :       tf_msg.transform.rotation    = odom->pose.pose.orientation;
+     329             :     }
+     330             : 
+     331      184540 :     if (Support::noNans(tf_msg)) {
+     332             :       try {
+     333      184497 :         broadcaster_->sendTransform(tf_msg);
+     334             :       }
+     335           0 :       catch (...) {
+     336           0 :         ROS_ERROR("exception caught ");
+     337             :       }
+     338             : 
+     339      184558 :       if (tf_from_attitude_enabled_) {
+     340      183504 :         if (is_inverted_) {
+     341      183503 :           tf_msg.child_frame_id += "_att_only";
+     342             :         } else {
+     343           1 :           tf_msg.header.frame_id += "_att_only";
+     344             :         }
+     345             :         try {
+     346      183506 :           broadcaster_->sendTransform(tf_msg);
+     347             :         }
+     348           0 :         catch (...) {
+     349           0 :           ROS_ERROR("exception caught ");
+     350             :         }
+     351             :       }
+     352             :       /*//}*/
+     353             : 
+     354             :       /*//{ tf utm origin */
+     355             : 
+     356      184562 :       geometry_msgs::TransformStamped tf_utm_msg;
+     357      184562 :       if (is_utm_source_) {
+     358             :         
+     359      166115 :         if (!is_utm_origin_set_) {
+     360           0 :           ROS_INFO_THROTTLE(5.0, "[%s]: %s utm_origin initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     361           0 :           return;
+     362             :         }
+     363             : 
+     364      166115 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     365      166115 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     366      166115 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     367             : 
+     368      166115 :         if (sh_altitude_amsl_.hasMsg()) {
+     369      166111 :           pose_utm.position.z = sh_altitude_amsl_.getMsg()->amsl;
+     370             :         } else {
+     371           4 :           ROS_WARN_THROTTLE(5.0, "[%s]: AMSL altitude not available.", getPrintName().c_str());
+     372             :         }
+     373             : 
+     374      166115 :         tf_utm_msg.header.stamp    = odom->header.stamp;
+     375      166115 :         tf_utm_msg.header.frame_id = ns_utm_origin_parent_frame_id_;
+     376      166115 :         tf_utm_msg.child_frame_id  = ns_utm_origin_child_frame_id_;
+     377             : 
+     378      166115 :         tf2::Transform tf_utm;
+     379      166115 :         if (is_inverted_) {
+     380      166115 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     381             :         } else {
+     382           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     383             :         }
+     384      166115 :         tf_utm_msg.transform = Support::msgFromTf2(tf_utm);
+     385             : 
+     386             :         try {
+     387      166115 :           broadcaster_->sendTransform(tf_utm_msg);
+     388      166115 :           ROS_INFO_ONCE("[%s]: publishing utm_origin tf", getPrintName().c_str());
+     389             :         }
+     390           0 :         catch (...) {
+     391           0 :           ROS_ERROR("exception caught ");
+     392             :         }
+     393             :       }
+     394             :       /*//}*/
+     395             : 
+     396             :       /*//{ tf world origin */
+     397      184557 :       if (is_utm_source_) {
+     398      332227 :         geometry_msgs::TransformStamped tf_world_msg;
+     399      166115 :         tf_world_msg.header.stamp    = odom->header.stamp;
+     400      166115 :         tf_world_msg.header.frame_id = ns_world_origin_parent_frame_id_;
+     401      166115 :         tf_world_msg.child_frame_id  = ns_world_origin_child_frame_id_;
+     402             : 
+     403      166115 :         tf2::Transform tf_world;
+     404      166115 :         tf_world.setOrigin(tf2::Vector3(world_origin_.x, world_origin_.y, world_origin_.z));
+     405      166115 :         tf_world.setRotation(tf2::Quaternion(0, 0, 0, 1));
+     406             : 
+     407      166115 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     408      166115 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     409      166115 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     410             : 
+     411      166115 :         tf2::Transform tf_utm;
+     412      166115 :         if (is_inverted_) {
+     413      166115 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     414             :         } else {
+     415           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     416             :         }
+     417             : 
+     418      166115 :         tf_world_msg.transform          = Support::msgFromTf2(tf_utm * tf_world);
+     419      166115 :         tf_world_msg.transform.rotation = pose_inv.orientation;
+     420             : 
+     421             :         try {
+     422      166115 :           broadcaster_->sendTransform(tf_world_msg);
+     423      166115 :           ROS_INFO_ONCE("[%s]: publishing world_origin tf", getPrintName().c_str());
+     424             :         }
+     425           0 :         catch (...) {
+     426           0 :           ROS_ERROR("exception caught ");
+     427             :         }
+     428             :       }
+     429             : 
+     430             :       /*//}*/
+     431             : 
+     432             :     } else {
+     433           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     434             :                         tf_msg.child_frame_id.c_str());
+     435             :     }
+     436      184558 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on topic: %s", getPrintName().c_str(),
+     437             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     438             :   }
+     439             :   /*//}*/
+     440             : 
+     441             :   /* publishTfFromAtt() //{*/
+     442      190527 :   void publishTfFromAtt(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     443             : 
+     444      571630 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     445             : 
+     446      381099 :     geometry_msgs::TransformStamped tf_msg;
+     447      190529 :     tf_msg.header.stamp = msg->header.stamp;
+     448             : 
+     449      190518 :     geometry_msgs::Pose pose;
+     450      190525 :     pose.position.x = 0.0;
+     451      190525 :     pose.position.y = 0.0;
+     452      190525 :     pose.position.z = 0.0;
+     453      190525 :     pose.orientation = msg->quaternion;
+     454      190542 :     if (is_inverted_) {
+     455             : 
+     456      190542 :       const tf2::Transform      tf       = Support::tf2FromPose(pose);
+     457      190528 :       const tf2::Transform      tf_inv   = tf.inverse();
+     458      190513 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     459             : 
+     460      190524 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     461      190524 :       tf_msg.child_frame_id        = msg->header.frame_id;
+     462      190524 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     463      190532 :       tf_msg.transform.rotation    = pose_inv.orientation;
+     464             : 
+     465             :     } else {
+     466           0 :       tf_msg.header.frame_id       = msg->header.frame_id;
+     467           0 :       tf_msg.child_frame_id        = ch_->frames.ns_fcu;
+     468           0 :       tf_msg.transform.translation = Support::pointToVector3(pose.position);
+     469           0 :       tf_msg.transform.rotation    = pose.orientation;
+     470             :     }
+     471             : 
+     472      190532 :     if (Support::noNans(tf_msg)) {
+     473             :       try {
+     474      190520 :         scope_timer.checkpoint("before pub");
+     475      190528 :         broadcaster_->sendTransform(tf_msg);
+     476      190552 :         scope_timer.checkpoint("after pub");
+     477             :       }
+     478           0 :       catch (...) {
+     479           0 :         ROS_ERROR("exception caught ");
+     480             :       }
+     481             :     } else {
+     482           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     483             :                         tf_msg.child_frame_id.c_str());
+     484             :     }
+     485      190552 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on topic: %s", getPrintName().c_str(),
+     486             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_attitude_.c_str());
+     487      190554 :   }
+     488             :   /*//}*/
+     489             : 
+     490             :   /* publishLocalTf() //{*/
+     491           0 :   void publishLocalTf(const std::string& frame_id) {
+     492             : 
+     493           0 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     494             : 
+     495           0 :     geometry_msgs::TransformStamped tf_msg;
+     496           0 :     tf_msg.header.stamp = ros::Time::now();
+     497             : 
+     498           0 :     tf_msg.header.frame_id       = frame_id;
+     499           0 :     tf_msg.child_frame_id        = frame_id.substr(0, frame_id.find("_origin")) + "_local_origin";
+     500           0 :     tf_msg.transform.translation = Support::pointToVector3(first_msg_->pose.pose.position);
+     501           0 :     tf_msg.transform.rotation    = first_msg_->pose.pose.orientation;
+     502             : 
+     503           0 :     if (Support::noNans(tf_msg)) {
+     504             :       try {
+     505           0 :         static_broadcaster_.sendTransform(tf_msg);
+     506             :       }
+     507           0 :       catch (...) {
+     508           0 :         ROS_ERROR("exception caught ");
+     509             :       }
+     510             :     } else {
+     511           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     512             :                         tf_msg.child_frame_id.c_str());
+     513             :     }
+     514           0 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     515             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     516           0 :     is_local_static_tf_published_ = true;
+     517           0 :   }
+     518             :   /*//}*/
+     519             : 
+     520             :   /* publishUtmTf() //{*/
+     521             :   void publishUtmTf(const std::string& frame_id) {
+     522             : 
+     523             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishUtmTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     524             : 
+     525             :     geometry_msgs::TransformStamped tf_msg;
+     526             :     tf_msg.header.stamp = ros::Time::now();
+     527             : 
+     528             :     tf_msg.header.frame_id         = frame_id;
+     529             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_utm_origin";
+     530             :     tf_msg.transform.translation.x = -utm_origin_.x;  // minus because inverse tf tree
+     531             :     tf_msg.transform.translation.y = -utm_origin_.y;  // minus because inverse tf tree
+     532             :     tf_msg.transform.translation.z = -utm_origin_.z;  // minus because inverse tf tree
+     533             :     tf_msg.transform.rotation.x    = 0;
+     534             :     tf_msg.transform.rotation.y    = 0;
+     535             :     tf_msg.transform.rotation.z    = 0;
+     536             :     tf_msg.transform.rotation.w    = 1;
+     537             : 
+     538             :     if (Support::noNans(tf_msg)) {
+     539             :       try {
+     540             :         static_broadcaster_.sendTransform(tf_msg);
+     541             :       }
+     542             :       catch (...) {
+     543             :         ROS_ERROR("exception caught ");
+     544             :       }
+     545             :     } else {
+     546             :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     547             :                         tf_msg.child_frame_id.c_str());
+     548             :     }
+     549             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     550             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     551             :     is_utm_static_tf_published_ = true;
+     552             :   }
+     553             :   /*//}*/
+     554             : 
+     555             :   /* publishWorldTf() //{*/
+     556             :   void publishWorldTf(const std::string& frame_id) {
+     557             : 
+     558             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishWorldTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     559             : 
+     560             :     geometry_msgs::TransformStamped tf_msg;
+     561             :     tf_msg.header.stamp = ros::Time::now();
+     562             : 
+     563             :     tf_msg.header.frame_id         = frame_id;
+     564             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_world_origin";
+     565             :     tf_msg.transform.translation.x = -(utm_origin_.x - world_origin_.x);  // minus because inverse tf tree
+     566             :     tf_msg.transform.translation.y = -(utm_origin_.y - world_origin_.y);  // minus because inverse tf tree
+     567             :     /* tf_msg.transform.translation.z = -(utm_origin_.z);                    // minus because inverse tf tree */
+     568             :     tf_msg.transform.translation.z = 0;
+     569             :     tf_msg.transform.rotation.x    = 0;
+     570             :     tf_msg.transform.rotation.y    = 0;
+     571             :     tf_msg.transform.rotation.z    = 0;
+     572             :     tf_msg.transform.rotation.w    = 1;
+     573             : 
+     574             :     if (Support::noNans(tf_msg)) {
+     575             :       try {
+     576             :         static_broadcaster_.sendTransform(tf_msg);
+     577             :       }
+     578             :       catch (...) {
+     579             :         ROS_ERROR("exception caught ");
+     580             :       }
+     581             :     } else {
+     582             :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     583             :                         tf_msg.child_frame_id.c_str());
+     584             :     }
+     585             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     586             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     587             :     is_world_static_tf_published_ = true;
+     588             :   }
+     589             :   /*//}*/
+     590             : 
+     591             :   /* republishInFrame() //{*/
+     592      139339 :   void republishInFrame(const nav_msgs::OdometryConstPtr& msg, const std::string& frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>& ph) {
+     593             : 
+     594      278678 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::republishInFrame", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     595             : 
+     596      139339 :     nav_msgs::Odometry msg_out = *msg;
+     597      139339 :     msg_out.header.frame_id    = frame_id;
+     598             : 
+     599      139339 :     geometry_msgs::PoseStamped pose;
+     600      139339 :     pose.header = msg->header;
+     601      139339 :     pose.pose   = msg->pose.pose;
+     602             : 
+     603      139339 :     auto res = ch_->transformer->transformSingle(pose, frame_id);
+     604      139339 :     if (res) {
+     605      131061 :       msg_out.pose.pose = res->pose;
+     606      131061 :       ph.publish(msg_out);
+     607             :     } else {
+     608        8278 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform pose to %s. Not republishing odom in this frame.", getPrintName().c_str(), frame_id.c_str());
+     609        8278 :       return;
+     610             :     }
+     611             :   }
+     612             : 
+     613             :   /*//}*/
+     614             : };
+     615             : 
+     616             : /*//}*/
+     617             : 
+     618             : }  // namespace mrs_uav_managers
+     619             : 
+     620             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html new file mode 100644 index 0000000000..bceb06a24c --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html @@ -0,0 +1,175 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..68724837bdbcf16b8715ed9373223639d78c7f83 GIT binary patch literal 2210 zcmV;T2wnGyP)4->0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpqkm zSgqOy=xwkq{pizrh2(NX3~Rk6*8qnYBrd5FgS!ci7*Vocp+1yjc2U_Bu z_UHMt|Lo~~c-?;xu)4&l*TOFIyk>HxyA^4MaRJVRV_FMS1Q`Ydfhnld7O4WHu`L2k zpvVR?$GQm&6om~EmN6>>#7;ofKomFWhT%TCL1MSJfTB79=mwNg*kit3#*Q23t0&>1 zbG&PWt}c_*il;)|jtDA5(4;gYyf2hyO94dx6cSHL&a1n>)?hm<9+F=3-Vv<9wYUvh zZ*4jCTE8&WH+Rj*aq?a&X&r!mr^6`tAzIhnz1~&ZN33-{C_X~##;*CU%FM*q|B%)$ z&^N&Xlk%Q|Ef#z?NRo{84HBnFF$Pwytv+WzVL{TQ<`lTVJcfpDZ+X@e0ds<_d?fQ7 zrmRuKTh|Iu>%x!lOkYZoTyxelY?4}YWLI2M)E_0}pYV+S94H^;G(G`={z5N~EaXkw z4A4W7j=4A|Dj;5&nY=&G`h`z`6q>|CwXSs8U`Ztp_Q+UJJgH0v zYh>=IMLBaQFuqj+&rs}EhEN?NR#4=G?dQOLJ}C?+^1z7K0IL*v!Ip5T&|to@9R(0y zs#3^pOtGKB?mq|mQZiKuIAF)td7%Tj zy>+8{t+?&!*$&TL9B30d4`x%}B7USdGll z&eRq8MFtuOEUVYpJVFaZarS{iVn~@I+v_^m@h;_f_k6N@8VuYa5O)xj8zcaPg97&l z0q&XN3oc4#e}6jA8{+r zx3HSVc_v3O^zaK!i0b10ZBZ<8l(iHUde#qRR5+j}DjZM~6;_`&?;vNtCS5_U^owER ztgq=yGtgA1<{sig@55)fl}vx~iBz?D?eewSj%16(FNPGDS8_7?fI|kCJV~Z&xz#fe zIF@@-pGJDBD)%*Cd$342o7#2;&xOL)0?<&Pfm&;n0TkIb6AbKGKKPv$0jgXWCs(nH z8J=7~>r&F8htuphvmh0wr$8s8377Oar0>erptywkQf=v4N1BTxz6c!-C}jYKVmW(w zV37g548sYa+VjE2ZTe&|g9J2g+eaz(%J|t!T((97_^%{V^#JBc(v!!Gn~xVEfB$A` zCf=;1W}KVqpFb z4IoaBNc+7|MmV$knHpzyNwY3x5RIn{d^EE}V$Dj=0Ckz-@l8^7)`;;>2(LYe5m^5` zCy{SG%>2aR?=*F*@MF?-_jGmHrLOtb4bTnU(1bpgbKfv`!vyJ9%iVlu_YlQQSN`%z z!t~_*A7E?W&U3tyflCGgvnCphE&P9lQmp@dJHE^Jc`|2$Tr)8~7&4_bg?XJT2H`oh%xGjCcyK_v$^3kTqGxm460G9cdj>7*h zXTMLVAhmKce8%LW+)U_wQ*$MHKqAebnEX`G1L*^UjPfoO78E})HLHQ@@y+4Y@y+42 zB>o9G{KP4Muakf;iqus;UU>?z-78O4(MO;*=~#UD0787GEA?Ir{gD)C#P4z$lX2rK z(HrCtYNXA>X6_qU+Q8OHh31&MsIZ%R2*J&3W8CF-PZjodiEVnpGd}uRsGQf=efdEz kXZ=`GvKAMt&Y>mpA42-e3MygS + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19523284.1 %
Date:2024-04-25 05:42:57Functions:88100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)1
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)3
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()94
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)97
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)2154
mrs_uav_managers::constraint_manager::ConstraintManager::stringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)2536
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)21996
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.func.html b/mrs_uav_managers/src/constraint_manager.cpp.func.html new file mode 100644 index 0000000000..ec5668d942 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19523284.1 %
Date:2024-04-25 05:42:57Functions:88100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)97
mrs_uav_managers::constraint_manager::ConstraintManager::stringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)2536
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)2154
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)3
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)21996
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)1
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()94
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/constraint_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..0360404b95 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.gcov.html b/mrs_uav_managers/src/constraint_manager.cpp.gcov.html new file mode 100644 index 0000000000..3ce1493315 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.html @@ -0,0 +1,716 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19523284.1 %
Date:2024-04-25 05:42:57Functions:88100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <nodelet/nodelet.h>
+       5             : 
+       6             : #include <std_msgs/String.h>
+       7             : 
+       8             : #include <mrs_msgs/ConstraintManagerDiagnostics.h>
+       9             : #include <mrs_msgs/EstimationDiagnostics.h>
+      10             : #include <mrs_msgs/DynamicsConstraintsSrv.h>
+      11             : #include <mrs_msgs/DynamicsConstraintsSrvRequest.h>
+      12             : #include <mrs_msgs/String.h>
+      13             : #include <mrs_msgs/ConstraintsOverride.h>
+      14             : 
+      15             : #include <mrs_lib/profiler.h>
+      16             : #include <mrs_lib/scope_timer.h>
+      17             : #include <mrs_lib/param_loader.h>
+      18             : #include <mrs_lib/mutex.h>
+      19             : #include <mrs_lib/publisher_handler.h>
+      20             : #include <mrs_lib/service_client_handler.h>
+      21             : #include <mrs_lib/subscribe_handler.h>
+      22             : 
+      23             : #include <dynamic_reconfigure/ReconfigureRequest.h>
+      24             : #include <dynamic_reconfigure/Reconfigure.h>
+      25             : #include <dynamic_reconfigure/Config.h>
+      26             : 
+      27             : //}
+      28             : 
+      29             : namespace mrs_uav_managers
+      30             : {
+      31             : 
+      32             : namespace constraint_manager
+      33             : {
+      34             : 
+      35             : /* //{ class ConstraintManager */
+      36             : 
+      37             : class ConstraintManager : public nodelet::Nodelet {
+      38             : 
+      39             : public:
+      40             :   virtual void onInit();
+      41             : 
+      42             : private:
+      43             :   ros::NodeHandle nh_;
+      44             : 
+      45             :   std::atomic<bool> is_initialized_ = false;
+      46             : 
+      47             :   // | ----------------------- parameters ----------------------- |
+      48             : 
+      49             :   std::vector<std::string> _estimator_type_names_;
+      50             : 
+      51             :   std::vector<std::string>                                       _constraint_names_;
+      52             :   std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest> _constraints_;
+      53             : 
+      54             :   std::map<std::string, std::vector<std::string>> _map_type_allowed_constraints_;
+      55             :   std::map<std::string, std::string>              _map_type_default_constraints_;
+      56             : 
+      57             :   // | --------------------- service clients -------------------- |
+      58             : 
+      59             :   mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> sc_set_constraints_;
+      60             : 
+      61             :   // | ----------------------- subscribers ---------------------- |
+      62             : 
+      63             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estimation_diag_;
+      64             : 
+      65             :   // | ------------- constraint management ------------- |
+      66             : 
+      67             :   bool setConstraints(std::string constraints_names);
+      68             : 
+      69             :   ros::ServiceServer service_server_set_constraints_;
+      70             :   bool               callbackSetConstraints(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+      71             : 
+      72             :   std::string last_estimator_name_;
+      73             :   std::mutex  mutex_last_estimator_name_;
+      74             : 
+      75             :   void       timerConstraintManagement(const ros::TimerEvent& event);
+      76             :   ros::Timer timer_constraint_management_;
+      77             :   double     _constraint_management_rate_;
+      78             : 
+      79             :   std::string current_constraints_;
+      80             :   std::mutex  mutex_current_constraints_;
+      81             : 
+      82             :   // | ------------------ constraints override ------------------ |
+      83             : 
+      84             :   ros::ServiceServer service_server_constraints_override_;
+      85             :   bool               callbackConstraintsOverride(mrs_msgs::ConstraintsOverride::Request& req, mrs_msgs::ConstraintsOverride::Response& res);
+      86             : 
+      87             :   std::atomic<bool>                    override_constraints_         = false;
+      88             :   std::atomic<bool>                    constraints_override_updated_ = false;
+      89             :   std::mutex                           mutex_constraints_override_;
+      90             :   mrs_msgs::ConstraintsOverrideRequest constraints_override_;
+      91             : 
+      92             :   // | ------------------ diagnostics publisher ----------------- |
+      93             : 
+      94             :   mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics> ph_diagnostics_;
+      95             : 
+      96             :   void       timerDiagnostics(const ros::TimerEvent& event);
+      97             :   ros::Timer timer_diagnostics_;
+      98             :   double     _diagnostics_rate_;
+      99             : 
+     100             :   // | ------------------------ profiler ------------------------ |
+     101             : 
+     102             :   mrs_lib::Profiler profiler_;
+     103             :   bool              _profiler_enabled_ = false;
+     104             : 
+     105             :   // | ------------------- scope timer logger ------------------- |
+     106             : 
+     107             :   bool                                       scope_timer_enabled_ = false;
+     108             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     109             : 
+     110             :   // | ------------------------- helpers ------------------------ |
+     111             : 
+     112             :   bool stringInVector(const std::string& value, const std::vector<std::string>& vector);
+     113             : };
+     114             : 
+     115             : //}
+     116             : 
+     117             : /* //{ onInit() */
+     118             : 
+     119          94 : void ConstraintManager::onInit() {
+     120             : 
+     121          94 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     122             : 
+     123          94 :   ros::Time::waitForValid();
+     124             : 
+     125          94 :   ROS_INFO("[ConstraintManager]: initializing");
+     126             : 
+     127             :   // | ------------------------- params ------------------------- |
+     128             : 
+     129         188 :   mrs_lib::ParamLoader param_loader(nh_, "ConstraintManager");
+     130             : 
+     131         188 :   std::string custom_config_path;
+     132         188 :   std::string platform_config_path;
+     133             : 
+     134          94 :   param_loader.loadParam("custom_config", custom_config_path);
+     135          94 :   param_loader.loadParam("platform_config", platform_config_path);
+     136             : 
+     137          94 :   if (custom_config_path != "") {
+     138          94 :     param_loader.addYamlFile(custom_config_path);
+     139             :   }
+     140             : 
+     141          94 :   if (platform_config_path != "") {
+     142          94 :     param_loader.addYamlFile(platform_config_path);
+     143             :   }
+     144             : 
+     145          94 :   param_loader.addYamlFileFromParam("private_config");
+     146          94 :   param_loader.addYamlFileFromParam("public_config");
+     147          94 :   param_loader.addYamlFileFromParam("public_constraints");
+     148             : 
+     149         188 :   const std::string yaml_prefix = "mrs_uav_managers/constraint_manager/";
+     150             : 
+     151             :   // params passed from the launch file are not prefixed
+     152          94 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154          94 :   param_loader.loadParam(yaml_prefix + "constraints", _constraint_names_);
+     155             : 
+     156          94 :   param_loader.loadParam(yaml_prefix + "estimator_types", _estimator_type_names_);
+     157             : 
+     158          94 :   param_loader.loadParam(yaml_prefix + "rate", _constraint_management_rate_);
+     159          94 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     160             : 
+     161          94 :   std::vector<std::string>::iterator it;
+     162             : 
+     163             :   // loading constraint names
+     164         376 :   for (it = _constraint_names_.begin(); it != _constraint_names_.end(); ++it) {
+     165             : 
+     166         282 :     ROS_INFO_STREAM("[ConstraintManager]: loading constraints '" << *it << "'");
+     167             : 
+     168         282 :     mrs_msgs::DynamicsConstraintsSrvRequest new_constraints;
+     169             : 
+     170         282 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/speed", new_constraints.constraints.horizontal_speed);
+     171         282 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/acceleration", new_constraints.constraints.horizontal_acceleration);
+     172         282 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/jerk", new_constraints.constraints.horizontal_jerk);
+     173         282 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/snap", new_constraints.constraints.horizontal_snap);
+     174             : 
+     175         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/speed", new_constraints.constraints.vertical_ascending_speed);
+     176         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/acceleration", new_constraints.constraints.vertical_ascending_acceleration);
+     177         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/jerk", new_constraints.constraints.vertical_ascending_jerk);
+     178         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/snap", new_constraints.constraints.vertical_ascending_snap);
+     179             : 
+     180         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/speed", new_constraints.constraints.vertical_descending_speed);
+     181         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/acceleration", new_constraints.constraints.vertical_descending_acceleration);
+     182         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/jerk", new_constraints.constraints.vertical_descending_jerk);
+     183         282 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/snap", new_constraints.constraints.vertical_descending_snap);
+     184             : 
+     185         282 :     param_loader.loadParam(yaml_prefix + *it + "/heading/speed", new_constraints.constraints.heading_speed);
+     186         282 :     param_loader.loadParam(yaml_prefix + *it + "/heading/acceleration", new_constraints.constraints.heading_acceleration);
+     187         282 :     param_loader.loadParam(yaml_prefix + *it + "/heading/jerk", new_constraints.constraints.heading_jerk);
+     188         282 :     param_loader.loadParam(yaml_prefix + *it + "/heading/snap", new_constraints.constraints.heading_snap);
+     189             : 
+     190         282 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/roll", new_constraints.constraints.roll_rate);
+     191         282 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/pitch", new_constraints.constraints.pitch_rate);
+     192         282 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/yaw", new_constraints.constraints.yaw_rate);
+     193             : 
+     194             :     double tilt_deg;
+     195             : 
+     196         282 :     param_loader.loadParam(yaml_prefix + *it + "/tilt", tilt_deg);
+     197             : 
+     198         282 :     new_constraints.constraints.tilt = M_PI * (tilt_deg / 180.0);
+     199             : 
+     200         282 :     _constraints_.insert(std::pair<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>(*it, new_constraints));
+     201             :   }
+     202             : 
+     203             :   // loading the allowed constraints lists
+     204         752 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     205             : 
+     206         658 :     std::vector<std::string> temp_vector;
+     207         658 :     param_loader.loadParam(yaml_prefix + "allowed_constraints/" + *it, temp_vector);
+     208             : 
+     209         658 :     std::vector<std::string>::iterator it2;
+     210        2432 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     211        1774 :       if (!stringInVector(*it2, _constraint_names_)) {
+     212           0 :         ROS_ERROR("[ConstraintManager]: the element '%s' of %s/allowed_constraints is not a valid constraint!", it2->c_str(), it->c_str());
+     213           0 :         ros::shutdown();
+     214             :       }
+     215             :     }
+     216             : 
+     217         658 :     _map_type_allowed_constraints_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     218             :   }
+     219             : 
+     220             :   // loading the default constraints
+     221         752 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     222             : 
+     223         658 :     std::string temp_str;
+     224         658 :     param_loader.loadParam(yaml_prefix + "default_constraints/" + *it, temp_str);
+     225             : 
+     226         658 :     if (!stringInVector(temp_str, _map_type_allowed_constraints_.at(*it))) {
+     227           0 :       ROS_ERROR("[ConstraintManager]: the element '%s' of %s/allowed_constraints is not a valid constraint!", temp_str.c_str(), it->c_str());
+     228           0 :       ros::shutdown();
+     229             :     }
+     230             : 
+     231         658 :     _map_type_default_constraints_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     232             :   }
+     233             : 
+     234          94 :   ROS_INFO("[ConstraintManager]: done loading dynamic params");
+     235             : 
+     236          94 :   current_constraints_ = "";
+     237          94 :   last_estimator_name_ = "";
+     238             : 
+     239             :   // | ------------------------ services ------------------------ |
+     240             : 
+     241          94 :   service_server_set_constraints_ = nh_.advertiseService("set_constraints_in", &ConstraintManager::callbackSetConstraints, this);
+     242             : 
+     243          94 :   service_server_constraints_override_ = nh_.advertiseService("constraints_override_in", &ConstraintManager::callbackConstraintsOverride, this);
+     244             : 
+     245          94 :   sc_set_constraints_ = mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>(nh_, "set_constraints_out");
+     246             : 
+     247             :   // | ----------------------- subscribers ---------------------- |
+     248             : 
+     249         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+     250          94 :   shopts.nh                 = nh_;
+     251          94 :   shopts.node_name          = "ConstraintManager";
+     252          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     253          94 :   shopts.threadsafe         = true;
+     254          94 :   shopts.autostart          = true;
+     255          94 :   shopts.queue_size         = 10;
+     256          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     257             : 
+     258          94 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     259             : 
+     260             :   // | ----------------------- publishers ----------------------- |
+     261             : 
+     262          94 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics>(nh_, "diagnostics_out", 10);
+     263             : 
+     264             :   // | ------------------------- timers ------------------------- |
+     265             : 
+     266          94 :   timer_constraint_management_ = nh_.createTimer(ros::Rate(_constraint_management_rate_), &ConstraintManager::timerConstraintManagement, this);
+     267          94 :   timer_diagnostics_           = nh_.createTimer(ros::Rate(_diagnostics_rate_), &ConstraintManager::timerDiagnostics, this);
+     268             : 
+     269             :   // | ------------------------ profiler ------------------------ |
+     270             : 
+     271          94 :   profiler_ = mrs_lib::Profiler(nh_, "ConstraintManager", _profiler_enabled_);
+     272             : 
+     273             :   // | ------------------- scope timer logger ------------------- |
+     274             : 
+     275          94 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     276         282 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     277          94 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     278             : 
+     279             :   // | ----------------------- finish init ---------------------- |
+     280             : 
+     281          94 :   if (!param_loader.loadedSuccessfully()) {
+     282           0 :     ROS_ERROR("[ConstraintManager]: Could not load all parameters!");
+     283           0 :     ros::shutdown();
+     284             :   }
+     285             : 
+     286          94 :   is_initialized_ = true;
+     287             : 
+     288          94 :   ROS_INFO("[ConstraintManager]: initialized");
+     289             : 
+     290          94 :   ROS_DEBUG("[ConstraintManager]: debug output is enabled");
+     291          94 : }
+     292             : 
+     293             : //}
+     294             : 
+     295             : // --------------------------------------------------------------
+     296             : // |                           methods                          |
+     297             : // --------------------------------------------------------------
+     298             : 
+     299             : /* setConstraints() //{ */
+     300             : 
+     301          97 : bool ConstraintManager::setConstraints(std::string constraints_name) {
+     302             : 
+     303          97 :   std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     304          97 :   it = _constraints_.find(constraints_name);
+     305             : 
+     306          97 :   if (it == _constraints_.end()) {
+     307           0 :     ROS_ERROR("[ConstraintManager]: could not setConstraints(), the constraint name '%s' is not on the list", constraints_name.c_str());
+     308           0 :     return false;
+     309             :   }
+     310             : 
+     311         194 :   mrs_msgs::DynamicsConstraintsSrv srv_call;
+     312             : 
+     313          97 :   srv_call.request = it->second;
+     314             : 
+     315          97 :   if (override_constraints_) {
+     316             : 
+     317           1 :     auto constraints_override = mrs_lib::get_mutexed(mutex_constraints_override_, constraints_override_);
+     318             : 
+     319           1 :     if (constraints_override.acceleration_horizontal > 0 &&
+     320           1 :         constraints_override.acceleration_horizontal <= srv_call.request.constraints.horizontal_acceleration) {
+     321           1 :       srv_call.request.constraints.horizontal_acceleration = constraints_override.acceleration_horizontal;
+     322             :     } else {
+     323           0 :       ROS_ERROR_THROTTLE(1.0, "[ConstraintManager]: required horizontal acceleration override is out of bounds");
+     324             :     }
+     325             : 
+     326           1 :     if (constraints_override.acceleration_vertical > 0 &&
+     327           1 :         constraints_override.acceleration_vertical <= srv_call.request.constraints.vertical_ascending_acceleration &&
+     328           1 :         constraints_override.acceleration_vertical <= srv_call.request.constraints.vertical_descending_acceleration) {
+     329           1 :       srv_call.request.constraints.vertical_ascending_acceleration  = constraints_override.acceleration_vertical;
+     330           1 :       srv_call.request.constraints.vertical_descending_acceleration = constraints_override.acceleration_vertical;
+     331             :     } else {
+     332           0 :       ROS_ERROR_THROTTLE(1.0, "[ConstraintManager]: required vertical acceleration override is out of bounds");
+     333             :     }
+     334             :   }
+     335             : 
+     336          97 :   bool res = sc_set_constraints_.call(srv_call);
+     337             : 
+     338          97 :   if (!res) {
+     339             : 
+     340           0 :     ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the service for setting constraints has failed!");
+     341           0 :     return false;
+     342             : 
+     343             :   } else {
+     344             : 
+     345          97 :     if (srv_call.response.success) {
+     346             : 
+     347          97 :       mrs_lib::set_mutexed(mutex_current_constraints_, constraints_name, current_constraints_);
+     348          97 :       return true;
+     349             : 
+     350             :     } else {
+     351             : 
+     352           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: set service for setting constraints returned: '%s'", srv_call.response.message.c_str());
+     353           0 :       return false;
+     354             :     }
+     355             :   }
+     356             : }
+     357             : 
+     358             : //}
+     359             : 
+     360             : // --------------------------------------------------------------
+     361             : // |                          callbacks                         |
+     362             : // --------------------------------------------------------------
+     363             : 
+     364             : // | -------------------- service callbacks ------------------- |
+     365             : 
+     366             : /* //{ callbackSetConstraints() */
+     367             : 
+     368           3 : bool ConstraintManager::callbackSetConstraints(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+     369             : 
+     370           3 :   if (!is_initialized_) {
+     371           0 :     return false;
+     372             :   }
+     373             : 
+     374           6 :   std::stringstream ss;
+     375             : 
+     376           3 :   if (!sh_estimation_diag_.hasMsg()) {
+     377             : 
+     378           0 :     ss << "missing odometry diagnostics";
+     379             : 
+     380           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     381             : 
+     382           0 :     res.message = ss.str();
+     383           0 :     res.success = false;
+     384           0 :     return true;
+     385             :   }
+     386             : 
+     387           6 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     388             : 
+     389           3 :   if (!stringInVector(req.value, _constraint_names_)) {
+     390             : 
+     391           1 :     ss << "the constraints '" << req.value.c_str() << "' do not exist (in the ConstraintManager's config)";
+     392             : 
+     393           1 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     394             : 
+     395           1 :     res.message = ss.str();
+     396           1 :     res.success = false;
+     397           1 :     return true;
+     398             :   }
+     399             : 
+     400           2 :   if (!stringInVector(req.value, _map_type_allowed_constraints_.at(estimation_diagnostics->current_state_estimator))) {
+     401             : 
+     402           0 :     ss << "the constraints '" << req.value.c_str() << "' are not allowed given the current odometry type";
+     403             : 
+     404           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     405             : 
+     406           0 :     res.message = ss.str();
+     407           0 :     res.success = false;
+     408           0 :     return true;
+     409             :   }
+     410             : 
+     411           2 :   override_constraints_ = false;
+     412             : 
+     413             :   // try to set the constraints
+     414           2 :   if (!setConstraints(req.value)) {
+     415             : 
+     416           0 :     ss << "the ControlManager could not set the constraints";
+     417             : 
+     418           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     419             : 
+     420           0 :     res.message = ss.str();
+     421           0 :     res.success = false;
+     422           0 :     return true;
+     423             : 
+     424             :   } else {
+     425             : 
+     426           2 :     ss << "the constraints '" << req.value.c_str() << "' were set";
+     427             : 
+     428           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     429             : 
+     430           2 :     res.message = ss.str();
+     431           2 :     res.success = true;
+     432           2 :     return true;
+     433             :   }
+     434             : }
+     435             : 
+     436             : //}
+     437             : 
+     438             : /* callackConstraintsOverride() //{ */
+     439             : 
+     440           1 : bool ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverride::Request& req, mrs_msgs::ConstraintsOverride::Response& res) {
+     441             : 
+     442           1 :   if (!is_initialized_) {
+     443           0 :     return false;
+     444             :   }
+     445             : 
+     446             :   {
+     447           1 :     std::scoped_lock lock(mutex_constraints_override_);
+     448             : 
+     449           1 :     constraints_override_ = req;
+     450             :   }
+     451             : 
+     452           1 :   override_constraints_         = true;
+     453           1 :   constraints_override_updated_ = true;
+     454             : 
+     455           1 :   ROS_INFO_THROTTLE(0.1, "[ConstraintManager]: setting constraints override");
+     456             : 
+     457           1 :   res.message = "override set";
+     458           1 :   res.success = true;
+     459             : 
+     460           1 :   return true;
+     461             : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : // --------------------------------------------------------------
+     466             : // |                           timers                           |
+     467             : // --------------------------------------------------------------
+     468             : 
+     469             : /* timerConstraintManagement() //{ */
+     470             : 
+     471       21996 : void ConstraintManager::timerConstraintManagement(const ros::TimerEvent& event) {
+     472             : 
+     473       21996 :   if (!is_initialized_) {
+     474        4855 :     return;
+     475             :   }
+     476             : 
+     477       43992 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerConstraintManagement", _constraint_management_rate_, 0.01, event);
+     478       43992 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerConstraintManagement", scope_timer_logger_, scope_timer_enabled_);
+     479             : 
+     480       21996 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     481       21996 :   auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_);
+     482             : 
+     483       21996 :   if (!sh_estimation_diag_.hasMsg()) {
+     484        4855 :     return;
+     485             :   }
+     486             : 
+     487       17141 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     488             : 
+     489             :   // | --- automatically set constraints when the state estimator changes -- |
+     490       17141 :   if (estimation_diagnostics->current_state_estimator != last_estimator_name_) {
+     491             : 
+     492          99 :     ROS_INFO_THROTTLE(1.0, "[ConstraintManager]: the state estimator has changed! %s -> %s", last_estimator_name_.c_str(),
+     493             :                       estimation_diagnostics->current_state_estimator.c_str());
+     494             : 
+     495          99 :     std::map<std::string, std::string>::iterator it;
+     496          99 :     it = _map_type_default_constraints_.find(estimation_diagnostics->current_state_estimator);
+     497             : 
+     498          99 :     if (it == _map_type_default_constraints_.end()) {
+     499             : 
+     500           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the state estimator type '%s' was not specified in the constraint_manager's config!",
+     501             :                         estimation_diagnostics->current_state_estimator.c_str());
+     502             : 
+     503             :     } else {
+     504             : 
+     505             :       // if the current constraints are within the allowed state estimator types, do nothing
+     506          99 :       if (stringInVector(current_constraints, _map_type_allowed_constraints_.at(estimation_diagnostics->current_state_estimator))) {
+     507             : 
+     508           5 :         last_estimator_name = estimation_diagnostics->current_state_estimator;
+     509             : 
+     510             :         // else, try to set the initial constraints
+     511             :       } else {
+     512             : 
+     513          94 :         ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the current constraints '%s' are not within the allowed constraints for '%s'", current_constraints.c_str(),
+     514             :                           estimation_diagnostics->current_state_estimator.c_str());
+     515             : 
+     516          94 :         if (setConstraints(it->second)) {
+     517             : 
+     518          94 :           last_estimator_name = estimation_diagnostics->current_state_estimator;
+     519             : 
+     520          94 :           ROS_INFO_THROTTLE(1.0, "[ConstraintManager]: constraints set to initial: '%s'", it->second.c_str());
+     521             : 
+     522             :         } else {
+     523             : 
+     524           0 :           ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: could not set constraints!");
+     525             :         }
+     526             :       }
+     527             :     }
+     528             :   }
+     529             : 
+     530       17141 :   if (constraints_override_updated_) {
+     531             : 
+     532           1 :     std::map<std::string, std::string>::iterator it;
+     533           1 :     it = _map_type_default_constraints_.find(last_estimator_name_);
+     534             : 
+     535           1 :     ROS_INFO_THROTTLE(0.1, "[ConstraintManager]: re-setting constraints with user value override");
+     536             : 
+     537           1 :     if (setConstraints(it->second)) {
+     538           1 :       constraints_override_updated_ = false;
+     539             :     } else {
+     540           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: could not re-set the constraints!");
+     541             :     }
+     542             :   }
+     543             : 
+     544       17141 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     545             : }
+     546             : 
+     547             : //}
+     548             : 
+     549             : /* timerDiagnostics() //{ */
+     550             : 
+     551        2154 : void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) {
+     552             : 
+     553        2154 :   if (!is_initialized_) {
+     554         470 :     return;
+     555             :   }
+     556             : 
+     557        4308 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     558        4308 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     559             : 
+     560        2154 :   if (!sh_estimation_diag_.hasMsg()) {
+     561         469 :     ROS_WARN_THROTTLE(10.0, "[ConstraintManager]: can not do constraint management, missing estimation diagnostics!");
+     562         469 :     return;
+     563             :   }
+     564             : 
+     565        1685 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     566             : 
+     567        1685 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     568             : 
+     569        1685 :   if (current_constraints == "") {  // this could happend just before timerConstraintManagement() finishes
+     570           1 :     return;
+     571             :   }
+     572             : 
+     573        1684 :   mrs_msgs::ConstraintManagerDiagnostics diagnostics;
+     574             : 
+     575        1684 :   diagnostics.stamp        = ros::Time::now();
+     576        1684 :   diagnostics.current_name = current_constraints;
+     577        1684 :   diagnostics.loaded       = _constraint_names_;
+     578             : 
+     579             :   // get the available constraints
+     580             :   {
+     581        1684 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     582        1684 :     it = _map_type_allowed_constraints_.find(estimation_diagnostics->current_state_estimator);
+     583             : 
+     584        1684 :     if (it == _map_type_allowed_constraints_.end()) {
+     585           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the state estimator '%s' was not specified in the constraint_manager's config!",
+     586             :                         estimation_diagnostics->current_state_estimator.c_str());
+     587             :     } else {
+     588        1684 :       diagnostics.available = it->second;
+     589             :     }
+     590             :   }
+     591             : 
+     592             :   // get the current constraint values
+     593             :   {
+     594        1684 :     std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     595        1684 :     it = _constraints_.find(current_constraints);
+     596             : 
+     597        1684 :     if (it == _constraints_.end()) {
+     598           0 :       ROS_ERROR("[ConstraintManager]: current constraints '%s' not found in the constraint list!", current_constraints.c_str());
+     599           0 :       return;
+     600             :     }
+     601             : 
+     602        1684 :     diagnostics.current_values = it->second.constraints;
+     603             :   }
+     604             : 
+     605        1684 :   ph_diagnostics_.publish(diagnostics);
+     606             : }
+     607             : 
+     608             : //}
+     609             : 
+     610             : // --------------------------------------------------------------
+     611             : // |                          routines                          |
+     612             : // --------------------------------------------------------------
+     613             : 
+     614             : /* stringInVector() //{ */
+     615             : 
+     616        2536 : bool ConstraintManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     617             : 
+     618        2536 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     619          95 :     return false;
+     620             :   } else {
+     621        2441 :     return true;
+     622             :   }
+     623             : }
+     624             : 
+     625             : //}
+     626             : 
+     627             : }  // namespace constraint_manager
+     628             : 
+     629             : }  // namespace mrs_uav_managers
+     630             : 
+     631             : #include <pluginlib/class_list_macros.h>
+     632          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::constraint_manager::ConstraintManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..38d8adfdaf --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html @@ -0,0 +1,178 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.gcov.png b/mrs_uav_managers/src/constraint_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8665e49273d3bd913d1e1e1695b4808b41ea8464 GIT binary patch literal 2190 zcmV;92yyp`P)>5)BTY8BblC`-Z9K!!Xe{bN>uLlw|>SUz(xi-s1E) z4_E^UP>2+y95r)9jOskK*@rm8ML1)(YqHRq;|bP}9Fh3vIsOyidnJY^)QH3&=I{g| z!Mjcg$TBndz$c@5Y)YLXkHERZCu;UQk7(mc&d?LiatUd~C^@+q zI|N<4$wa8B z>SV?^hp@{P;mJEBL~~%m2vG7p6I7Z(TL$Dfdc=!?Hx(J;gftSln=_nDBUK(DqFP1* z5p5sXz5DdIT(MYg(7~@-2NY%w|1EvO5Ff$}Ac#o1jHv9D2(#v?N24%+p@_QViPbc& z&l6`JUl-24D>V#p+sp=sdC+5GJ|_q_Yd31xc0@R8gl&jP2knT2<6{3(`@*AS58A3( z>SP=%CFCgE6`tYO2_fYc3FT8Ktr@#JCre1qT?#)vIEb83VQXO73X?34<>I=5M|NaW zvvgKUh%r@e=6uD!Qh~^-5T~$`=kqKe{Jj*r{FUQ236l+DDA*B*c#U-<;yq2fWXrzdVaFj4!AQ_>EN6)8*aHKFhain*32!7*TM1!Y6dJ-|ma$@CjF8YM zc|6Owf#(!cjYlVLQctrUcQhA*&sUoW+bxsK0mcoPRM=-yaXZtxC-#|gr_pViNNVRP zlzfZ{)08&(W7E`req!!}(4yJ=-j@ty0wKzqfmU!8J3aUDg~xmUIbOeB@AM%ed{)Lm z_^uH|!tv*%Yzcsj`c00@@pzZxx)m=VL>lRKT(q5$-3{a1OyXrnz(@ zOo)LH{Z6VU(-2E&JtEuh$)~;~^oSR|y#QKTu_2TC7N_XD9P$ux*umr2hVL=RrRsPD z2HXcnsMuX^ie^aLMW5glppnNR=Uf+=0#dlmJ|hhTmjYm&tgqWFGp9>s?NXQiLiL*` zBAamu&1?U0AA~GLl+EmfvTR0^&O`w&(?X%bnWLcsydBZ{A_kSh2ca__o1SG?#T91g@cojl1;sfoctOvChxqx3|EF$DeNL<}dEFI9TC$u%K zq^Y0CA>Y~CSA2I**-WbkYp0@~toNZJO zd%|mD->|rDcy#B-B$`Facx$4GuNLClxJ{Q8XIyvpr>|`43O5F2TQMERRBNuEkQVZt zFlD4D6978ysdj}|wvm#mk_Y=DVTa+zA3-!BzTX^k6yY`E>#9|7ms4aXL1+;Sa0-}{ zw>ibJyM`mG<6YVS{UxCDG}QK-yTYpipb4Qri@Q3ELxk6e*ZcbIS1&g{TB_HzTYLMv zxCqbpF#&kBuWOizd;GU2)FJ+vb&VEv#m%JNbL_+Mo@XU9$i1g>aks^LLdWG@?hpU! zt{~;{p2CL>&0A_fw})nU) z^gB+@+?e~ngQXJo;%;Y@&wU8P-WdP{o^fXYc;u%^c)$w-f!F>kTHvt=PWg6I5Xz?( zw@5x|nZZ4PvkMr%=_6cVTjCSOt}!^%7cYX6VW@7*A5pY7{&y1_=V34`I6f45WDcjRl=$B&KOaERxMF ztWlk<$J#Jor)XuzUYuXolG^8FQ8@X>4&%n3RE!JueJzsQ$Ks{Td=*|Gih6eCu{6_M zY`srWdSmGGefT~FJ+k||zpUmObrq1QXW%2ECU}obJ^$A;bzxLR1#;2wHa@9V1%T{3Sp-3@>^my00Ib0L@AH%c1XzI z*UIzS`mQUL_*?ie(8sDcVR81>6lcOT^O5(E2|IgXWG;T&w=*YHm9b$#;j9go@~Rbi>WBR2bE2>AydIWcin zh7MnICS}uKVa3^S+U)SJoMRH!J^$(<@-k-^LWd*v^O%x}wazBH5oBTJKSg~oJ5v4u QtpET307*qoM6N<$g5-=RhyVZp literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.func-sort-c.html b/mrs_uav_managers/src/control_manager/common/common.cpp.func-sort-c.html new file mode 100644 index 0000000000..da239a07fe --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.func-sort-c.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/common - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::validateOdometry(nav_msgs::Odometry_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, double const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)1
mrs_uav_managers::control_manager::loadDetailedUavModelParams(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_uav_managers::control_manager::validateHwApiPositionCmd(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)494
mrs_uav_managers::control_manager::validateVelocityReference(mrs_msgs::VelocityReference_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)720
mrs_uav_managers::control_manager::validateHwApiAccelerationHdgRateCmd(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1003
mrs_uav_managers::control_manager::validateHwApiAccelerationHdgCmd(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1062
mrs_uav_managers::control_manager::validateHwApiVelocityHdgCmd(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1314
mrs_uav_managers::control_manager::idxInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)1467
mrs_uav_managers::control_manager::validateHwApiVelocityHdgRateCmd(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2329
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2780
mrs_uav_managers::control_manager::validateHwApiActuatorCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3776
mrs_uav_managers::control_manager::validateHwApiControlGroupCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3779
mrs_uav_managers::control_manager::validateReference(mrs_msgs::Reference_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10476
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)14301
mrs_uav_managers::control_manager::initializeDefaultOutput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)14302
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)14780
mrs_uav_managers::control_manager::validateHwApiAttitudeRateCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)78490
mrs_uav_managers::control_manager::validateTrackerCommand(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)90555
mrs_uav_managers::control_manager::validateControlOutput(mrs_uav_managers::Controller::ControlOutput const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)100275
mrs_uav_managers::control_manager::validateHwApiAttitudeCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)108302
mrs_uav_managers::control_manager::validateUavState(mrs_msgs::UavState_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)138590
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)147072
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.func.html b/mrs_uav_managers/src/control_manager/common/common.cpp.func.html new file mode 100644 index 0000000000..15848c2854 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.func.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/common - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::idxInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)1467
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)14780
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)147072
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2780
mrs_uav_managers::control_manager::validateOdometry(nav_msgs::Odometry_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::validateUavState(mrs_msgs::UavState_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)138590
mrs_uav_managers::control_manager::validateReference(mrs_msgs::Reference_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10476
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, double const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)14301
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)1
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::validateControlOutput(mrs_uav_managers::Controller::ControlOutput const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)100275
mrs_uav_managers::control_manager::validateTrackerCommand(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)90555
mrs_uav_managers::control_manager::initializeDefaultOutput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)14302
mrs_uav_managers::control_manager::validateHwApiActuatorCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3776
mrs_uav_managers::control_manager::validateHwApiAttitudeCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)108302
mrs_uav_managers::control_manager::validateHwApiPositionCmd(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)494
mrs_uav_managers::control_manager::validateVelocityReference(mrs_msgs::VelocityReference_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)720
mrs_uav_managers::control_manager::loadDetailedUavModelParams(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_uav_managers::control_manager::validateHwApiVelocityHdgCmd(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1314
mrs_uav_managers::control_manager::validateHwApiAttitudeRateCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)78490
mrs_uav_managers::control_manager::validateHwApiControlGroupCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3779
mrs_uav_managers::control_manager::validateHwApiAccelerationHdgCmd(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1062
mrs_uav_managers::control_manager::validateHwApiVelocityHdgRateCmd(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2329
mrs_uav_managers::control_manager::validateHwApiAccelerationHdgRateCmd(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1003
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.frameset.html b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.frameset.html new file mode 100644 index 0000000000..41b7eb37bb --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html new file mode 100644 index 0000000000..eb3900b2e6 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html @@ -0,0 +1,1312 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/common - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_managers/control_manager/common.h>
+       2             : 
+       3             : namespace mrs_uav_managers
+       4             : {
+       5             : 
+       6             : namespace control_manager
+       7             : {
+       8             : 
+       9             : /* idxInVector() //{ */
+      10             : 
+      11        1467 : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec) {
+      12             : 
+      13        4139 :   for (unsigned int i = 0; i < vec.size(); i++) {
+      14        4135 :     if (str == vec[i]) {
+      15        1463 :       return {i};
+      16             :     }
+      17             :   }
+      18             : 
+      19           4 :   return {};
+      20             : }
+      21             : 
+      22             : //}
+      23             : 
+      24             : /* validateTrackerCommand() //{ */
+      25             : 
+      26       90555 : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name) {
+      27             : 
+      28       90555 :   if (!msg) {
+      29           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: the optional variable '%s' is not set!!!", node_name.c_str(), var_name.c_str());
+      30           0 :     return false;
+      31             :   }
+      32             : 
+      33             :   // check positions
+      34             : 
+      35       90555 :   if (!std::isfinite(msg->position.x)) {
+      36           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->position.x'!!!", node_name.c_str(), var_name.c_str());
+      37           0 :     return false;
+      38             :   }
+      39             : 
+      40       90555 :   if (!std::isfinite(msg->position.y)) {
+      41           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->position.y'!!!", node_name.c_str(), var_name.c_str());
+      42           0 :     return false;
+      43             :   }
+      44             : 
+      45       90555 :   if (!std::isfinite(msg->position.z)) {
+      46           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->position.z'!!!", node_name.c_str(), var_name.c_str());
+      47           0 :     return false;
+      48             :   }
+      49             : 
+      50             :   // check velocities
+      51             : 
+      52       90555 :   if (!std::isfinite(msg->velocity.x)) {
+      53           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->velocity.x'!!!", node_name.c_str(), var_name.c_str());
+      54           0 :     return false;
+      55             :   }
+      56             : 
+      57       90555 :   if (!std::isfinite(msg->velocity.y)) {
+      58           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->velocity.y'!!!", node_name.c_str(), var_name.c_str());
+      59           0 :     return false;
+      60             :   }
+      61             : 
+      62       90555 :   if (!std::isfinite(msg->velocity.z)) {
+      63           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->velocity.z'!!!", node_name.c_str(), var_name.c_str());
+      64           0 :     return false;
+      65             :   }
+      66             : 
+      67             :   // check accelerations
+      68             : 
+      69       90555 :   if (!std::isfinite(msg->acceleration.x)) {
+      70           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+      71           0 :     return false;
+      72             :   }
+      73             : 
+      74       90555 :   if (!std::isfinite(msg->acceleration.y)) {
+      75           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+      76           0 :     return false;
+      77             :   }
+      78             : 
+      79       90555 :   if (!std::isfinite(msg->acceleration.z)) {
+      80           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+      81           0 :     return false;
+      82             :   }
+      83             : 
+      84             :   // check jerk
+      85             : 
+      86       90555 :   if (!std::isfinite(msg->jerk.x)) {
+      87           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->jerk.x'!!!", node_name.c_str(), var_name.c_str());
+      88           0 :     return false;
+      89             :   }
+      90             : 
+      91       90555 :   if (!std::isfinite(msg->jerk.y)) {
+      92           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->jerk.y'!!!", node_name.c_str(), var_name.c_str());
+      93           0 :     return false;
+      94             :   }
+      95             : 
+      96       90555 :   if (!std::isfinite(msg->jerk.z)) {
+      97           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->jerk.z'!!!", node_name.c_str(), var_name.c_str());
+      98           0 :     return false;
+      99             :   }
+     100             : 
+     101             :   // check snap
+     102             : 
+     103       90555 :   if (!std::isfinite(msg->snap.x)) {
+     104           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->snap.x'!!!", node_name.c_str(), var_name.c_str());
+     105           0 :     return false;
+     106             :   }
+     107             : 
+     108       90555 :   if (!std::isfinite(msg->snap.y)) {
+     109           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->snap.y'!!!", node_name.c_str(), var_name.c_str());
+     110           0 :     return false;
+     111             :   }
+     112             : 
+     113       90555 :   if (!std::isfinite(msg->snap.z)) {
+     114           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->snap.z'!!!", node_name.c_str(), var_name.c_str());
+     115           0 :     return false;
+     116             :   }
+     117             : 
+     118             :   // check attitude rate
+     119             : 
+     120       90555 :   if (!std::isfinite(msg->attitude_rate.x)) {
+     121           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->attitude_rate.x'!!!", node_name.c_str(), var_name.c_str());
+     122           0 :     return false;
+     123             :   }
+     124             : 
+     125       90555 :   if (!std::isfinite(msg->attitude_rate.y)) {
+     126           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->attitude_rate.y'!!!", node_name.c_str(), var_name.c_str());
+     127           0 :     return false;
+     128             :   }
+     129             : 
+     130       90555 :   if (!std::isfinite(msg->attitude_rate.z)) {
+     131           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->attitude_rate.z'!!!", node_name.c_str(), var_name.c_str());
+     132           0 :     return false;
+     133             :   }
+     134             : 
+     135             :   // check heading
+     136             : 
+     137       90555 :   if (!std::isfinite(msg->heading)) {
+     138           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->heading'!!!", node_name.c_str(), var_name.c_str());
+     139           0 :     return false;
+     140             :   }
+     141             : 
+     142       90555 :   if (!std::isfinite(msg->heading_rate)) {
+     143           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     144           0 :     return false;
+     145             :   }
+     146             : 
+     147       90555 :   if (!std::isfinite(msg->heading_acceleration)) {
+     148           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->heading_acceleration'!!!", node_name.c_str(), var_name.c_str());
+     149           0 :     return false;
+     150             :   }
+     151             : 
+     152       90555 :   if (!std::isfinite(msg->heading_jerk)) {
+     153           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->heading_jerk'!!!", node_name.c_str(), var_name.c_str());
+     154           0 :     return false;
+     155             :   }
+     156             : 
+     157             :   // check throttle
+     158             : 
+     159       90555 :   if (!std::isfinite(msg->throttle)) {
+     160           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s->throttle'!!!", node_name.c_str(), var_name.c_str());
+     161           0 :     return false;
+     162             :   }
+     163             : 
+     164       90555 :   return true;
+     165             : }
+     166             : 
+     167             : //}
+     168             : 
+     169             : /* validateOdometry() //{ */
+     170             : 
+     171           0 : bool validateOdometry(const nav_msgs::Odometry& msg, const std::string& node_name, const std::string& var_name) {
+     172             : 
+     173             :   // check position
+     174             : 
+     175           0 :   if (!std::isfinite(msg.pose.pose.position.x)) {
+     176           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.position.x'!!!", node_name.c_str(), var_name.c_str());
+     177           0 :     return false;
+     178             :   }
+     179             : 
+     180           0 :   if (!std::isfinite(msg.pose.pose.position.y)) {
+     181           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.position.y'!!!", node_name.c_str(), var_name.c_str());
+     182           0 :     return false;
+     183             :   }
+     184             : 
+     185           0 :   if (!std::isfinite(msg.pose.pose.position.z)) {
+     186           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.position.z'!!!", node_name.c_str(), var_name.c_str());
+     187           0 :     return false;
+     188             :   }
+     189             : 
+     190             :   // check orientation
+     191             : 
+     192           0 :   if (!std::isfinite(msg.pose.pose.orientation.x)) {
+     193           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.x'!!!", node_name.c_str(), var_name.c_str());
+     194           0 :     return false;
+     195             :   }
+     196             : 
+     197           0 :   if (!std::isfinite(msg.pose.pose.orientation.y)) {
+     198           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.y'!!!", node_name.c_str(), var_name.c_str());
+     199           0 :     return false;
+     200             :   }
+     201             : 
+     202           0 :   if (!std::isfinite(msg.pose.pose.orientation.z)) {
+     203           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.z'!!!", node_name.c_str(), var_name.c_str());
+     204           0 :     return false;
+     205             :   }
+     206             : 
+     207           0 :   if (!std::isfinite(msg.pose.pose.orientation.w)) {
+     208           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.pose.orientation.w'!!!", node_name.c_str(), var_name.c_str());
+     209           0 :     return false;
+     210             :   }
+     211             : 
+     212             :   // check velocity
+     213             : 
+     214           0 :   if (!std::isfinite(msg.twist.twist.linear.x)) {
+     215           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.twist.twist.linear.x'!!!", node_name.c_str(), var_name.c_str());
+     216           0 :     return false;
+     217             :   }
+     218             : 
+     219           0 :   if (!std::isfinite(msg.twist.twist.linear.y)) {
+     220           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.twist.twist.linear.y'!!!", node_name.c_str(), var_name.c_str());
+     221           0 :     return false;
+     222             :   }
+     223             : 
+     224           0 :   if (!std::isfinite(msg.twist.twist.linear.z)) {
+     225           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.twist.twist.linear.z'!!!", node_name.c_str(), var_name.c_str());
+     226           0 :     return false;
+     227             :   }
+     228             : 
+     229           0 :   return true;
+     230             : }
+     231             : 
+     232             : //}
+     233             : 
+     234             : /* validateVelocityReference() //{ */
+     235             : 
+     236         720 : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name) {
+     237             : 
+     238             :   // check velocity
+     239             : 
+     240         720 :   if (!std::isfinite(msg.velocity.x)) {
+     241           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     242           0 :     return false;
+     243             :   }
+     244             : 
+     245         720 :   if (!std::isfinite(msg.velocity.y)) {
+     246           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     247           0 :     return false;
+     248             :   }
+     249             : 
+     250         720 :   if (!std::isfinite(msg.velocity.z)) {
+     251           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     252           0 :     return false;
+     253             :   }
+     254             : 
+     255         720 :   if (!std::isfinite(msg.altitude)) {
+     256           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.altitude'!!!", node_name.c_str(), var_name.c_str());
+     257           0 :     return false;
+     258             :   }
+     259             : 
+     260         720 :   if (!std::isfinite(msg.heading)) {
+     261           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     262           0 :     return false;
+     263             :   }
+     264             : 
+     265         720 :   if (!std::isfinite(msg.heading_rate)) {
+     266           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     267           0 :     return false;
+     268             :   }
+     269             : 
+     270         720 :   return true;
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* validateReference() //{ */
+     276             : 
+     277       10476 : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name) {
+     278             : 
+     279             :   // check position
+     280             : 
+     281       10476 :   if (!std::isfinite(msg.position.x)) {
+     282           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.x'!!!", node_name.c_str(), var_name.c_str());
+     283           0 :     return false;
+     284             :   }
+     285             : 
+     286       10476 :   if (!std::isfinite(msg.position.y)) {
+     287           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.y'!!!", node_name.c_str(), var_name.c_str());
+     288           0 :     return false;
+     289             :   }
+     290             : 
+     291       10476 :   if (!std::isfinite(msg.position.z)) {
+     292           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.z'!!!", node_name.c_str(), var_name.c_str());
+     293           0 :     return false;
+     294             :   }
+     295             : 
+     296             :   // check heading
+     297             : 
+     298       10476 :   if (!std::isfinite(msg.heading)) {
+     299           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     300           0 :     return false;
+     301             :   }
+     302             : 
+     303       10476 :   return true;
+     304             : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* validateUavState() //{ */
+     309             : 
+     310      138590 : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_name) {
+     311             : 
+     312             :   // check position
+     313             : 
+     314      138590 :   if (!std::isfinite(msg.pose.position.x)) {
+     315           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.position.x'!!!", node_name.c_str(), var_name.c_str());
+     316           0 :     return false;
+     317             :   }
+     318             : 
+     319      138590 :   if (!std::isfinite(msg.pose.position.y)) {
+     320           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.position.y'!!!", node_name.c_str(), var_name.c_str());
+     321           0 :     return false;
+     322             :   }
+     323             : 
+     324      138590 :   if (!std::isfinite(msg.pose.position.z)) {
+     325           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.position.z'!!!", node_name.c_str(), var_name.c_str());
+     326           0 :     return false;
+     327             :   }
+     328             : 
+     329             :   // check orientation
+     330             : 
+     331      138590 :   if (!std::isfinite(msg.pose.orientation.x)) {
+     332           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.orientation.x'!!!", node_name.c_str(), var_name.c_str());
+     333           0 :     return false;
+     334             :   }
+     335             : 
+     336      138590 :   if (!std::isfinite(msg.pose.orientation.y)) {
+     337           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.orientation.y'!!!", node_name.c_str(), var_name.c_str());
+     338           0 :     return false;
+     339             :   }
+     340             : 
+     341      138590 :   if (!std::isfinite(msg.pose.orientation.z)) {
+     342           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.orientation.z'!!!", node_name.c_str(), var_name.c_str());
+     343           0 :     return false;
+     344             :   }
+     345             : 
+     346      138590 :   if (!std::isfinite(msg.pose.orientation.w)) {
+     347           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pose.orientation.w'!!!", node_name.c_str(), var_name.c_str());
+     348           0 :     return false;
+     349             :   }
+     350             : 
+     351             :   // check linear velocity
+     352             : 
+     353      138590 :   if (!std::isfinite(msg.velocity.linear.x)) {
+     354           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.linear.x'!!!", node_name.c_str(), var_name.c_str());
+     355           0 :     return false;
+     356             :   }
+     357             : 
+     358      138590 :   if (!std::isfinite(msg.velocity.linear.y)) {
+     359           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.linear.y'!!!", node_name.c_str(), var_name.c_str());
+     360           0 :     return false;
+     361             :   }
+     362             : 
+     363      138590 :   if (!std::isfinite(msg.velocity.linear.z)) {
+     364           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.linear.z'!!!", node_name.c_str(), var_name.c_str());
+     365           0 :     return false;
+     366             :   }
+     367             : 
+     368             :   // check angular velocity
+     369             : 
+     370      138590 :   if (!std::isfinite(msg.velocity.angular.x)) {
+     371           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.angular.x'!!!", node_name.c_str(), var_name.c_str());
+     372           0 :     return false;
+     373             :   }
+     374             : 
+     375      138590 :   if (!std::isfinite(msg.velocity.angular.y)) {
+     376           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.angular.y'!!!", node_name.c_str(), var_name.c_str());
+     377           0 :     return false;
+     378             :   }
+     379             : 
+     380      138590 :   if (!std::isfinite(msg.velocity.angular.z)) {
+     381           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.angular.z'!!!", node_name.c_str(), var_name.c_str());
+     382           0 :     return false;
+     383             :   }
+     384             : 
+     385             :   // check linear acceleration
+     386             : 
+     387      138590 :   if (!std::isfinite(msg.acceleration.linear.x)) {
+     388           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.linear.x'!!!", node_name.c_str(), var_name.c_str());
+     389           0 :     return false;
+     390             :   }
+     391             : 
+     392      138590 :   if (!std::isfinite(msg.acceleration.linear.y)) {
+     393           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.linear.y'!!!", node_name.c_str(), var_name.c_str());
+     394           0 :     return false;
+     395             :   }
+     396             : 
+     397      138590 :   if (!std::isfinite(msg.acceleration.linear.z)) {
+     398           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.linear.z'!!!", node_name.c_str(), var_name.c_str());
+     399           0 :     return false;
+     400             :   }
+     401             : 
+     402             :   // check angular acceleration
+     403             : 
+     404      138590 :   if (!std::isfinite(msg.acceleration.angular.x)) {
+     405           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.angular.x'!!!", node_name.c_str(), var_name.c_str());
+     406           0 :     return false;
+     407             :   }
+     408             : 
+     409      138590 :   if (!std::isfinite(msg.acceleration.angular.y)) {
+     410           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.angular.y'!!!", node_name.c_str(), var_name.c_str());
+     411           0 :     return false;
+     412             :   }
+     413             : 
+     414      138590 :   if (!std::isfinite(msg.acceleration.angular.z)) {
+     415           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.angular.z'!!!", node_name.c_str(), var_name.c_str());
+     416           0 :     return false;
+     417             :   }
+     418             : 
+     419             :   // check acceleration angular disturbance
+     420             : 
+     421      138590 :   if (!std::isfinite(msg.acceleration_disturbance.angular.x)) {
+     422           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration_disturbance.angular.x'!!!", node_name.c_str(), var_name.c_str());
+     423           0 :     return false;
+     424             :   }
+     425             : 
+     426      138590 :   if (!std::isfinite(msg.acceleration_disturbance.angular.y)) {
+     427           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration_disturbance.angular.y'!!!", node_name.c_str(), var_name.c_str());
+     428           0 :     return false;
+     429             :   }
+     430             : 
+     431      138590 :   if (!std::isfinite(msg.acceleration_disturbance.angular.z)) {
+     432           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration_disturbance.angular.z'!!!", node_name.c_str(), var_name.c_str());
+     433           0 :     return false;
+     434             :   }
+     435             : 
+     436             :   // check acceleration linear disturbance
+     437             : 
+     438      138590 :   if (!std::isfinite(msg.acceleration_disturbance.linear.x)) {
+     439           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration_disturbance.linear.x'!!!", node_name.c_str(), var_name.c_str());
+     440           0 :     return false;
+     441             :   }
+     442             : 
+     443      138590 :   if (!std::isfinite(msg.acceleration_disturbance.linear.y)) {
+     444           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration_disturbance.linear.y'!!!", node_name.c_str(), var_name.c_str());
+     445           0 :     return false;
+     446             :   }
+     447             : 
+     448      138590 :   if (!std::isfinite(msg.acceleration_disturbance.linear.z)) {
+     449           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration_disturbance.linear.z'!!!", node_name.c_str(), var_name.c_str());
+     450           0 :     return false;
+     451             :   }
+     452             : 
+     453      138590 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* RCChannelToRange() //{ */
+     459             : 
+     460        2780 : double RCChannelToRange(double rc_value, double range, double deadband) {
+     461             : 
+     462        2780 :   double tmp_neg1_to_1 = (rc_value - 0.5) * 2.0;
+     463             : 
+     464        2780 :   if (tmp_neg1_to_1 > 1.0) {
+     465           0 :     tmp_neg1_to_1 = 1.0;
+     466        2780 :   } else if (tmp_neg1_to_1 < -1.0) {
+     467           0 :     tmp_neg1_to_1 = -1.0;
+     468             :   }
+     469             : 
+     470             :   // check the deadband
+     471        2780 :   if (tmp_neg1_to_1 < deadband && tmp_neg1_to_1 > -deadband) {
+     472        2149 :     return 0.0;
+     473             :   }
+     474             : 
+     475         631 :   if (tmp_neg1_to_1 > 0) {
+     476             : 
+     477         416 :     double tmp = (tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     478             : 
+     479         416 :     return range * tmp;
+     480             : 
+     481             :   } else {
+     482             : 
+     483         215 :     double tmp = (-tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     484             : 
+     485         215 :     return -range * tmp;
+     486             :   }
+     487             : 
+     488             :   return 0.0;
+     489             : }
+     490             : 
+     491             : //}
+     492             : 
+     493             : /* loadDetailedUavModelParams() //{ */
+     494             : 
+     495          94 : std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name, const std::string& platform_config,
+     496             :                                                                 const std::string& custom_config) {
+     497             : 
+     498         188 :   mrs_lib::ParamLoader param_loader(nh, node_name);
+     499             : 
+     500          94 :   if (custom_config != "") {
+     501          94 :     param_loader.addYamlFile(custom_config);
+     502             :   }
+     503             : 
+     504          94 :   if (platform_config != "") {
+     505          94 :     param_loader.addYamlFile(platform_config);
+     506             :   }
+     507             : 
+     508             :   double mass;
+     509             :   double arm_length;
+     510             :   double body_height;
+     511             :   double force_constant;
+     512             :   double torque_constant;
+     513             :   double prop_radius;
+     514             :   double rpm_min;
+     515             :   double rpm_max;
+     516             : 
+     517          94 :   bool detailed_loaded = true;
+     518             : 
+     519          94 :   detailed_loaded *= param_loader.loadParam("uav_mass", mass, 0.0);
+     520             : 
+     521          94 :   detailed_loaded *= param_loader.loadParam("model_params/arm_length", arm_length, 0.0);
+     522          94 :   detailed_loaded *= param_loader.loadParam("model_params/body_height", body_height, 0.0);
+     523             : 
+     524          94 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/force_constant", force_constant, 0.0);
+     525          94 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant, 0.0);
+     526          94 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius, 0.0);
+     527          94 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min, 0.0);
+     528          94 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max, 0.0);
+     529             : 
+     530         188 :   Eigen::MatrixXd allocation_matrix;
+     531             : 
+     532          94 :   detailed_loaded *= param_loader.loadMatrixDynamic("model_params/propulsion/allocation_matrix", allocation_matrix, Eigen::Matrix4d::Identity(), 4, -1);
+     533             : 
+     534          94 :   if (!detailed_loaded) {
+     535           0 :     ROS_WARN(
+     536             :         "[%s]: detailed UAV model params not loaded, missing some parameters. This will not permit operations when ACTUATORS or CONTROL_GROUP control "
+     537             :         "outputs would be possible.",
+     538             :         node_name.c_str());
+     539           0 :     return {};
+     540             :   } else {
+     541          94 :     ROS_INFO("[%s]: detailed UAV model params successfully loaded.", node_name.c_str());
+     542             :   }
+     543             : 
+     544          94 :   int n_motors = allocation_matrix.cols();
+     545             : 
+     546         188 :   DetailedModelParams_t model_params;
+     547             : 
+     548          94 :   model_params.arm_length  = arm_length;
+     549          94 :   model_params.body_height = body_height;
+     550          94 :   model_params.prop_radius = prop_radius;
+     551             : 
+     552          94 :   Eigen::Matrix3d inertia_matrix;
+     553             : 
+     554          94 :   bool inertia_loaded = param_loader.loadMatrixStatic("model_params/inertia_matrix", inertia_matrix, Eigen::Matrix3d::Identity());
+     555             : 
+     556          94 :   if (inertia_loaded) {
+     557             : 
+     558           0 :     model_params.inertia = inertia_matrix;
+     559           0 :     ROS_INFO("[%s]: inertia matrix loaded from config file:", node_name.c_str());
+     560           0 :     ROS_INFO_STREAM(model_params.inertia);
+     561             : 
+     562             :   } else {
+     563             : 
+     564          94 :     ROS_INFO("[%s]: inertia matrix missing in the config file, computing it from the other params.", node_name.c_str());
+     565             : 
+     566             :     // create the inertia matrix
+     567          94 :     model_params.inertia       = Eigen::Matrix3d::Zero();
+     568          94 :     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     569          94 :     model_params.inertia(1, 1) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     570          94 :     model_params.inertia(2, 2) = (mass * arm_length * arm_length) / 2.0;
+     571             : 
+     572          94 :     ROS_INFO("[%s]: inertia computed form parameters:", node_name.c_str());
+     573          94 :     ROS_INFO_STREAM(model_params.inertia);
+     574             :   }
+     575             : 
+     576             :   // create the force-torque allocation matrix
+     577          94 :   model_params.force_torque_mixer = allocation_matrix;
+     578          94 :   model_params.force_torque_mixer.row(0) *= arm_length * force_constant;
+     579          94 :   model_params.force_torque_mixer.row(1) *= arm_length * force_constant;
+     580          94 :   model_params.force_torque_mixer.row(2) *= torque_constant * (3.0 * prop_radius) * force_constant;
+     581          94 :   model_params.force_torque_mixer.row(3) *= force_constant;
+     582             : 
+     583             :   // | ------- create the control group allocation matrix ------- |
+     584             : 
+     585             :   // pseudoinverse of the force-torque matrix (maximum norm)
+     586             :   Eigen::MatrixXd alloc_tmp =
+     587         188 :       model_params.force_torque_mixer.transpose() * (model_params.force_torque_mixer * model_params.force_torque_mixer.transpose()).inverse();
+     588             : 
+     589             :   // | ------------- normalize the allocation matrix ------------ |
+     590             :   // this will make it match the PX4 control group mixing
+     591             : 
+     592             :   // the first two columns (roll, pitch)
+     593         476 :   for (int i = 0; i < n_motors; i++) {
+     594         382 :     alloc_tmp.block(i, 0, 1, 2).normalize();
+     595             :   }
+     596             : 
+     597             :   // the 3rd column (yaw)
+     598         476 :   for (int i = 0; i < n_motors; i++) {
+     599         382 :     if (alloc_tmp(i, 2) > 1e-2) {
+     600         191 :       alloc_tmp(i, 2) = 1.0;
+     601         191 :     } else if (alloc_tmp(i, 2) < -1e-2) {
+     602         191 :       alloc_tmp(i, 2) = -1.0;
+     603             :     } else {
+     604           0 :       alloc_tmp(i, 2) = 0.0;
+     605             :     }
+     606             :   }
+     607             : 
+     608             :   // the 4th column (throttle)
+     609         476 :   for (int i = 0; i < n_motors; i++) {
+     610         382 :     alloc_tmp(i, 3) = 1.0;
+     611             :   }
+     612             : 
+     613          94 :   model_params.control_group_mixer = alloc_tmp;
+     614             : 
+     615          94 :   return model_params;
+     616             : }
+     617             : 
+     618             : //}
+     619             : 
+     620             : /* getLowestOutput() //{ */
+     621             : 
+     622       14780 : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs) {
+     623             : 
+     624       14780 :   if (outputs.actuators) {
+     625          16 :     return ACTUATORS_CMD;
+     626             :   }
+     627             : 
+     628       14764 :   if (outputs.control_group) {
+     629          16 :     return CONTROL_GROUP;
+     630             :   }
+     631             : 
+     632       14748 :   if (outputs.attitude_rate) {
+     633       14641 :     return ATTITUDE_RATE;
+     634             :   }
+     635             : 
+     636         107 :   if (outputs.attitude) {
+     637          16 :     return ATTITUDE;
+     638             :   }
+     639             : 
+     640          91 :   if (outputs.acceleration_hdg_rate) {
+     641          10 :     return ACCELERATION_HDG_RATE;
+     642             :   }
+     643             : 
+     644          81 :   if (outputs.acceleration_hdg) {
+     645          10 :     return ACCELERATION_HDG;
+     646             :   }
+     647             : 
+     648          71 :   if (outputs.velocity_hdg_rate) {
+     649          41 :     return VELOCITY_HDG_RATE;
+     650             :   }
+     651             : 
+     652          30 :   if (outputs.velocity_hdg) {
+     653          20 :     return VELOCITY_HDG;
+     654             :   }
+     655             : 
+     656          10 :   return POSITION;
+     657             : }
+     658             : 
+     659             : //}
+     660             : 
+     661             : /* getHighestOutput() //{ */
+     662             : 
+     663           0 : CONTROL_OUTPUT getHighestOuput(const ControlOutputModalities_t& outputs) {
+     664             : 
+     665           0 :   if (outputs.position) {
+     666           0 :     return POSITION;
+     667             :   }
+     668             : 
+     669           0 :   if (outputs.velocity_hdg) {
+     670           0 :     return VELOCITY_HDG;
+     671             :   }
+     672             : 
+     673           0 :   if (outputs.velocity_hdg_rate) {
+     674           0 :     return VELOCITY_HDG_RATE;
+     675             :   }
+     676             : 
+     677           0 :   if (outputs.acceleration_hdg) {
+     678           0 :     return ACCELERATION_HDG;
+     679             :   }
+     680             : 
+     681           0 :   if (outputs.acceleration_hdg_rate) {
+     682           0 :     return ACCELERATION_HDG_RATE;
+     683             :   }
+     684             : 
+     685           0 :   if (outputs.attitude) {
+     686           0 :     return ATTITUDE;
+     687             :   }
+     688             : 
+     689           0 :   if (outputs.attitude_rate) {
+     690           0 :     return ATTITUDE_RATE;
+     691             :   }
+     692             : 
+     693           0 :   if (outputs.control_group) {
+     694           0 :     return CONTROL_GROUP;
+     695             :   }
+     696             : 
+     697           0 :   return ACTUATORS_CMD;
+     698             : }
+     699             : 
+     700             : //}
+     701             : 
+     702             : // | -------- extraction of throttle out of hw api cmds ------- |
+     703             : 
+     704             : /* extractThrottle() //{ */
+     705             : 
+     706      147072 : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output) {
+     707             : 
+     708      147072 :   if (!control_output.control_output) {
+     709       21321 :     return {};
+     710             :   }
+     711             : 
+     712      251502 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     713             : }
+     714             : 
+     715             : //}
+     716             : 
+     717             : // | -------------- validation of HW api commands ------------- |
+     718             : 
+     719             : /* validateControlOutput() //{ */
+     720             : 
+     721      100275 : bool validateControlOutput(const Controller::ControlOutput& control_output, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     722             :                            const std::string& var_name) {
+     723             : 
+     724      100275 :   if (!control_output.control_output) {
+     725           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: the optional variable '%s' is not set!!!", node_name.c_str(), var_name.c_str());
+     726           0 :     return false;
+     727             :   }
+     728             : 
+     729      100275 :   std::variant<ControlOutputModalities_t> output_modalities_var{output_modalities};
+     730      200550 :   std::variant<std::string>               node_name_var{node_name};
+     731      100275 :   std::variant<std::string>               var_name_var{var_name};
+     732             : 
+     733      100275 :   return std::visit(HwApiValidateVisitor(), control_output.control_output.value(), output_modalities_var, node_name_var, var_name_var);
+     734             : }
+     735             : 
+     736             : //}
+     737             : 
+     738             : /* validateHwApiActuatorCmd() //{ */
+     739             : 
+     740        3776 : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name) {
+     741             : 
+     742       18880 :   for (size_t i = 0; i < msg.motors.size(); i++) {
+     743       15104 :     if (!std::isfinite(msg.motors[i])) {
+     744           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.motors[%d]'!!!", node_name.c_str(), var_name.c_str(), int(i));
+     745           0 :       return false;
+     746             :     }
+     747             :   }
+     748             : 
+     749        3776 :   return true;
+     750             : }
+     751             : 
+     752             : //}
+     753             : 
+     754             : /* validateHwApiControlGroupCmd() //{ */
+     755             : 
+     756        3779 : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name) {
+     757             : 
+     758        3779 :   if (!std::isfinite(msg.roll)) {
+     759           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.roll'!!!", node_name.c_str(), var_name.c_str());
+     760           0 :     return false;
+     761             :   }
+     762             : 
+     763        3779 :   if (!std::isfinite(msg.pitch)) {
+     764           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pitch'!!!", node_name.c_str(), var_name.c_str());
+     765           0 :     return false;
+     766             :   }
+     767             : 
+     768        3779 :   if (!std::isfinite(msg.yaw)) {
+     769           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.yaw'!!!", node_name.c_str(), var_name.c_str());
+     770           0 :     return false;
+     771             :   }
+     772             : 
+     773        3779 :   if (!std::isfinite(msg.throttle)) {
+     774           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     775           0 :     return false;
+     776             :   }
+     777             : 
+     778        3779 :   return true;
+     779             : }
+     780             : 
+     781             : //}
+     782             : 
+     783             : /* validateHwApiAttitudeCmd() //{ */
+     784             : 
+     785      108302 : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name) {
+     786             : 
+     787             :   // check the orientation
+     788             : 
+     789      108302 :   if (!std::isfinite(msg.orientation.x)) {
+     790           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.x'!!!", node_name.c_str(), var_name.c_str());
+     791           0 :     return false;
+     792             :   }
+     793             : 
+     794      108302 :   if (!std::isfinite(msg.orientation.y)) {
+     795           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.y'!!!", node_name.c_str(), var_name.c_str());
+     796           0 :     return false;
+     797             :   }
+     798             : 
+     799      108302 :   if (!std::isfinite(msg.orientation.z)) {
+     800           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.z'!!!", node_name.c_str(), var_name.c_str());
+     801           0 :     return false;
+     802             :   }
+     803             : 
+     804      108302 :   if (!std::isfinite(msg.orientation.w)) {
+     805           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.w'!!!", node_name.c_str(), var_name.c_str());
+     806           0 :     return false;
+     807             :   }
+     808             : 
+     809             :   // check the throttle
+     810             : 
+     811      108302 :   if (!std::isfinite(msg.throttle)) {
+     812           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     813           0 :     return false;
+     814             :   }
+     815             : 
+     816      108302 :   return true;
+     817             : }
+     818             : 
+     819             : //}
+     820             : 
+     821             : /* validateHwApiAttitudeRateCmd() //{ */
+     822             : 
+     823       78490 : bool validateHwApiAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     824             : 
+     825             :   // check the body rate
+     826             : 
+     827       78490 :   if (!std::isfinite(msg.body_rate.x)) {
+     828           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.x'!!!", node_name.c_str(), var_name.c_str());
+     829           0 :     return false;
+     830             :   }
+     831             : 
+     832       78490 :   if (!std::isfinite(msg.body_rate.y)) {
+     833           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.y'!!!", node_name.c_str(), var_name.c_str());
+     834           0 :     return false;
+     835             :   }
+     836             : 
+     837       78490 :   if (!std::isfinite(msg.body_rate.z)) {
+     838           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.z'!!!", node_name.c_str(), var_name.c_str());
+     839           0 :     return false;
+     840             :   }
+     841             : 
+     842             :   // check the throttle
+     843             : 
+     844       78490 :   if (!std::isfinite(msg.throttle)) {
+     845           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     846           0 :     return false;
+     847             :   }
+     848             : 
+     849       78490 :   return true;
+     850             : }
+     851             : 
+     852             : //}
+     853             : 
+     854             : /* validateHwApiAccelerationHdgRateCmd() //{ */
+     855             : 
+     856        1003 : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     857             : 
+     858             :   // | ----------------- check the acceleration ----------------- |
+     859             : 
+     860        1003 :   if (!std::isfinite(msg.acceleration.x)) {
+     861           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     862           0 :     return false;
+     863             :   }
+     864             : 
+     865        1003 :   if (!std::isfinite(msg.acceleration.y)) {
+     866           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     867           0 :     return false;
+     868             :   }
+     869             : 
+     870        1003 :   if (!std::isfinite(msg.acceleration.z)) {
+     871           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     872           0 :     return false;
+     873             :   }
+     874             : 
+     875             :   // check the heading rate
+     876             : 
+     877        1003 :   if (!std::isfinite(msg.heading_rate)) {
+     878           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     879           0 :     return false;
+     880             :   }
+     881             : 
+     882        1003 :   return true;
+     883             : }
+     884             : 
+     885             : //}
+     886             : 
+     887             : /* validateHwApiAccelerationHdgCmd() //{ */
+     888             : 
+     889        1062 : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     890             : 
+     891             :   // | ----------------- check the acceleration ----------------- |
+     892             : 
+     893        1062 :   if (!std::isfinite(msg.acceleration.x)) {
+     894           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     895           0 :     return false;
+     896             :   }
+     897             : 
+     898        1062 :   if (!std::isfinite(msg.acceleration.y)) {
+     899           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     900           0 :     return false;
+     901             :   }
+     902             : 
+     903        1062 :   if (!std::isfinite(msg.acceleration.z)) {
+     904           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     905           0 :     return false;
+     906             :   }
+     907             : 
+     908             :   // check the heading
+     909             : 
+     910        1062 :   if (!std::isfinite(msg.heading)) {
+     911           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     912           0 :     return false;
+     913             :   }
+     914             : 
+     915        1062 :   return true;
+     916             : }
+     917             : 
+     918             : //}
+     919             : 
+     920             : /* validateHwApiVelocityHdgRateCmd() //{ */
+     921             : 
+     922        2329 : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     923             : 
+     924             :   // | ----------------- check the velocity ----------------- |
+     925             : 
+     926        2329 :   if (!std::isfinite(msg.velocity.x)) {
+     927           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     928           0 :     return false;
+     929             :   }
+     930             : 
+     931        2329 :   if (!std::isfinite(msg.velocity.y)) {
+     932           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     933           0 :     return false;
+     934             :   }
+     935             : 
+     936        2329 :   if (!std::isfinite(msg.velocity.z)) {
+     937           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     938           0 :     return false;
+     939             :   }
+     940             : 
+     941             :   // check the heading rate
+     942             : 
+     943        2329 :   if (!std::isfinite(msg.heading_rate)) {
+     944           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     945           0 :     return false;
+     946             :   }
+     947             : 
+     948        2329 :   return true;
+     949             : }
+     950             : 
+     951             : //}
+     952             : 
+     953             : /* validateHwApiVelocityHdgCmd() //{ */
+     954             : 
+     955        1314 : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     956             : 
+     957             :   // | ----------------- check the velocity ----------------- |
+     958             : 
+     959        1314 :   if (!std::isfinite(msg.velocity.x)) {
+     960           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     961           0 :     return false;
+     962             :   }
+     963             : 
+     964        1314 :   if (!std::isfinite(msg.velocity.y)) {
+     965           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     966           0 :     return false;
+     967             :   }
+     968             : 
+     969        1314 :   if (!std::isfinite(msg.velocity.z)) {
+     970           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     971           0 :     return false;
+     972             :   }
+     973             : 
+     974             :   // check the heading
+     975             : 
+     976        1314 :   if (!std::isfinite(msg.heading)) {
+     977           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     978           0 :     return false;
+     979             :   }
+     980             : 
+     981        1314 :   return true;
+     982             : }
+     983             : 
+     984             : //}
+     985             : 
+     986             : /* validateHwApiPositionHdgCmd() //{ */
+     987             : 
+     988         494 : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name) {
+     989             : 
+     990             :   // | ----------------- check the position ----------------- |
+     991             : 
+     992         494 :   if (!std::isfinite(msg.position.x)) {
+     993           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.x'!!!", node_name.c_str(), var_name.c_str());
+     994           0 :     return false;
+     995             :   }
+     996             : 
+     997         494 :   if (!std::isfinite(msg.position.y)) {
+     998           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.y'!!!", node_name.c_str(), var_name.c_str());
+     999           0 :     return false;
+    1000             :   }
+    1001             : 
+    1002         494 :   if (!std::isfinite(msg.position.z)) {
+    1003           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.z'!!!", node_name.c_str(), var_name.c_str());
+    1004           0 :     return false;
+    1005             :   }
+    1006             : 
+    1007             :   // check the heading
+    1008             : 
+    1009         494 :   if (!std::isfinite(msg.heading)) {
+    1010           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+    1011           0 :     return false;
+    1012             :   }
+    1013             : 
+    1014         494 :   return true;
+    1015             : }
+    1016             : 
+    1017             : //}
+    1018             : 
+    1019             : // | ------------ initialization of HW api commands ----------- |
+    1020             : 
+    1021             : /* initializeDefaultOutput() //{ */
+    1022             : 
+    1023       14302 : Controller::HwApiOutputVariant initializeDefaultOutput(const ControlOutputModalities_t& possible_outputs, const mrs_msgs::UavState& uav_state,
+    1024             :                                                        const double& min_throttle, const double& n_motors) {
+    1025             : 
+    1026       14302 :   CONTROL_OUTPUT lowest_output = getLowestOuput(possible_outputs);
+    1027             : 
+    1028       14302 :   Controller::HwApiOutputVariant output;
+    1029             : 
+    1030       14302 :   switch (lowest_output) {
+    1031           0 :     case ACTUATORS_CMD: {
+    1032           0 :       output = mrs_msgs::HwApiActuatorCmd();
+    1033           0 :       break;
+    1034             :     }
+    1035           0 :     case CONTROL_GROUP: {
+    1036           0 :       output = mrs_msgs::HwApiControlGroupCmd();
+    1037           0 :       break;
+    1038             :     }
+    1039       14301 :     case ATTITUDE_RATE: {
+    1040       14301 :       output = mrs_msgs::HwApiAttitudeRateCmd();
+    1041       14301 :       break;
+    1042             :     }
+    1043           0 :     case ATTITUDE: {
+    1044           0 :       output = mrs_msgs::HwApiAttitudeCmd();
+    1045           0 :       break;
+    1046             :     }
+    1047           0 :     case ACCELERATION_HDG_RATE: {
+    1048           0 :       output = mrs_msgs::HwApiAccelerationHdgRateCmd();
+    1049           0 :       break;
+    1050             :     }
+    1051           0 :     case ACCELERATION_HDG: {
+    1052           0 :       output = mrs_msgs::HwApiAccelerationHdgCmd();
+    1053           0 :       break;
+    1054             :     }
+    1055           1 :     case VELOCITY_HDG_RATE: {
+    1056           1 :       output = mrs_msgs::HwApiVelocityHdgRateCmd();
+    1057           1 :       break;
+    1058             :     }
+    1059           0 :     case VELOCITY_HDG: {
+    1060           0 :       output = mrs_msgs::HwApiVelocityHdgCmd();
+    1061           0 :       break;
+    1062             :     }
+    1063           0 :     case POSITION: {
+    1064           0 :       output = mrs_msgs::HwApiPositionCmd();
+    1065           0 :       break;
+    1066             :     }
+    1067             :   }
+    1068             : 
+    1069       28604 :   std::variant<mrs_msgs::UavState> uav_state_var{uav_state};
+    1070       14302 :   std::variant<double>             min_throttle_var{min_throttle};
+    1071       14302 :   std::variant<double>             n_motors_var{n_motors};
+    1072             : 
+    1073       14302 :   std::visit(HwApiInitializeVisitor(), output, uav_state_var, min_throttle_var, n_motors_var);
+    1074             : 
+    1075       28604 :   return output;
+    1076             : }  // namespace mrs_uav_managers
+    1077             : 
+    1078             : //}
+    1079             : 
+    1080             : /* initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg) //{ */
+    1081             : 
+    1082           0 : void initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg, const double& min_throttle, const double& n_motors) {
+    1083             : 
+    1084           0 :   msg.stamp = ros::Time::now();
+    1085             : 
+    1086           0 :   for (int i = 0; i < n_motors; i++) {
+    1087           0 :     msg.motors.push_back(min_throttle);
+    1088             :   }
+    1089           0 : }
+    1090             : 
+    1091             : //}
+    1092             : 
+    1093             : /* initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg) //{ */
+    1094             : 
+    1095           0 : void initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg, const double& min_throttle) {
+    1096             : 
+    1097           0 :   msg.stamp = ros::Time::now();
+    1098             : 
+    1099           0 :   msg.roll     = 0;
+    1100           0 :   msg.pitch    = 0;
+    1101           0 :   msg.yaw      = 0;
+    1102           0 :   msg.throttle = min_throttle;
+    1103           0 : }
+    1104             : 
+    1105             : //}
+    1106             : 
+    1107             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg) //{ */
+    1108             : 
+    1109       14301 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle) {
+    1110             : 
+    1111       14301 :   msg.stamp = ros::Time::now();
+    1112             : 
+    1113       14301 :   msg.body_rate.x = 0;
+    1114       14301 :   msg.body_rate.y = 0;
+    1115       14301 :   msg.body_rate.z = 0;
+    1116       14301 :   msg.throttle    = min_throttle;
+    1117       14301 : }
+    1118             : 
+    1119             : //}
+    1120             : 
+    1121             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg) //{ */
+    1122             : 
+    1123           0 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle) {
+    1124             : 
+    1125           0 :   msg.stamp = ros::Time::now();
+    1126             : 
+    1127           0 :   msg.orientation = uav_state.pose.orientation;
+    1128           0 :   msg.throttle    = min_throttle;
+    1129           0 : }
+    1130             : 
+    1131             : //}
+    1132             : 
+    1133             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg) //{ */
+    1134             : 
+    1135           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1136             : 
+    1137           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1138           0 :   msg.header.stamp    = ros::Time::now();
+    1139             : 
+    1140           0 :   msg.acceleration.x = 0;
+    1141           0 :   msg.acceleration.y = 0;
+    1142           0 :   msg.acceleration.z = 0;
+    1143           0 :   msg.heading_rate   = 0;
+    1144           0 : }
+    1145             : 
+    1146             : //}
+    1147             : 
+    1148             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg) //{ */
+    1149             : 
+    1150           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1151             : 
+    1152           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1153           0 :   msg.header.stamp    = ros::Time::now();
+    1154             : 
+    1155           0 :   msg.acceleration.x = 0;
+    1156           0 :   msg.acceleration.y = 0;
+    1157           0 :   msg.acceleration.z = 0;
+    1158             : 
+    1159             :   try {
+    1160           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1161             :   }
+    1162           0 :   catch (std::runtime_error& exrun) {
+    1163           0 :     msg.heading = 0;
+    1164             :   }
+    1165           0 : }
+    1166             : 
+    1167             : //}
+    1168             : 
+    1169             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg) //{ */
+    1170             : 
+    1171           1 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1172             : 
+    1173           1 :   msg.header.frame_id = uav_state.header.frame_id;
+    1174           1 :   msg.header.stamp    = ros::Time::now();
+    1175             : 
+    1176           1 :   msg.velocity.x   = 0;
+    1177           1 :   msg.velocity.y   = 0;
+    1178           1 :   msg.velocity.z   = 0;
+    1179           1 :   msg.heading_rate = 0;
+    1180           1 : }
+    1181             : 
+    1182             : //}
+    1183             : 
+    1184             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg) //{ */
+    1185             : 
+    1186           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1187             : 
+    1188           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1189           0 :   msg.header.stamp    = ros::Time::now();
+    1190             : 
+    1191           0 :   msg.velocity.x = 0;
+    1192           0 :   msg.velocity.y = 0;
+    1193           0 :   msg.velocity.z = 0;
+    1194             : 
+    1195             :   try {
+    1196           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1197             :   }
+    1198           0 :   catch (std::runtime_error& exrun) {
+    1199           0 :     msg.heading = 0;
+    1200             :   }
+    1201           0 : }
+    1202             : 
+    1203             : //}
+    1204             : 
+    1205             : /* initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg) //{ */
+    1206             : 
+    1207           0 : void initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1208             : 
+    1209           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1210           0 :   msg.header.stamp    = ros::Time::now();
+    1211             : 
+    1212           0 :   msg.position.x = uav_state.pose.position.x;
+    1213           0 :   msg.position.y = uav_state.pose.position.y;
+    1214           0 :   msg.position.z = uav_state.pose.position.z;
+    1215             : 
+    1216             :   try {
+    1217           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1218             :   }
+    1219           0 :   catch (std::runtime_error& exrun) {
+    1220           0 :     msg.heading = 0;
+    1221             :   }
+    1222           0 : }
+    1223             : 
+    1224             : //}
+    1225             : 
+    1226             : }  // namespace control_manager
+    1227             : 
+    1228             : }  // namespace mrs_uav_managers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html new file mode 100644 index 0000000000..96cf02a42b --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html @@ -0,0 +1,327 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..bff27e0e5ac701165b83f61178c4e29c824fd4a5 GIT binary patch literal 2462 zcmV;P31Rk$P);z49%p-CbxmTg(6tug{dq5hd~r=Ivw57sC+hh&G23lY-`(>z;!Y=1t+xGZ9UZC;SJjc|^dZ5R(q zg#f-@f3B_D-}m-MJ#7!G{ZRpK&+ChBw^s_n0Om>j{)s;f+Y|^qW@K^kSRtwQOYsb5 zT4ly5ZYd{pDe(+uT4ly5ZYiIOhzAu=7vdSrw91TD+=4ORK|GOYoGG5)MpA>BR+$-! zJDBN`85=4*s)h>4TDFS`X)x0&GgfgkzLF`PNCJi{o_A|wjrplA#u`;zgrRPk>2_q? z^pWwMO!45;D_1=0Z^v5ZhEWDQX8KB;7s;A$O%4kjLi_YKzgo41b4RAQKE5E0fZ9-3 zx-i$lOsmX*lwHP`PKak?utV_-W(G4~U}j7ogPBg5kv*p|GJDSGy7CE)BIkj^6B@{g z4YB{nKcVrBg%6&8=t3U{Gu<$AsVrB|sdywYgpN%oYIe!YN2$Z9{siQn%|xW1$AnZ) z9|tpSGV_IvoW5s;OFGh0DP+%)&hW|eqm4OITqnCQ*TGDy%#=?dq;iH%x;^H}asV>Abm}!+6JAD+3eJ*9!gPB&D@jPx3pL_Z9W`mg)nDIPr5uY0q zPiWa)h-Wajou~+#yjzDHj7extK6Ugf0y3z-_Zqts)?0M`|ruMmZjFf8D z^SJ4pf+rrwm--2sE3;nG2FSDpJ};0)s;X0lQYdDG$GcPz`N<3b^Ds(%O#m`6-nOao zo-yJKIr?1t7PdGe%jX(1Q6I3lQrw=*o8J{Grqw=rS-?LdQ!hFpC&$_rYh$MG6!F?wdq0n9w}!N9n1yRHK$G63_rHZ2BNUXFZ>786+yQOCIH z_*LE|RqMEIaks)1pZdG%b)Yds8Nen}TyDxFtWbB%pm9CipTYqrZw!S#^kgfXntzrm z^Ni}#8)7t{k6tz4d98_hBR@f6fnnZ2@s8^~dwL*08JFL_bqxi4?lk zY_5$$+?`(N6@;Z`&nd{nf=QrhY28mR3lt8EDC#Zb#4d!Gbv7zmfdxoMxQGt}tYTbu z%>^lr9A-?ucL$Bmb51D)oow0*wu3yTG$s5#mya{^i7uk3*J7?z@i3 zC27sv9!=$-kxe6=Rd6&Wx~?t9xGX(6QnIX1BQr0gco&T$e^dBRv|) zuIo&33`^*3LMm6wP6b>btpO7eGEwo)N2!OR_GFeb@|<=J(|I2L-eT*x?mD--M+QG8?qOi87xx^R&w3|u&mH7GjfUB~Qy-~WDNPH- zEijoMHc9+)51tzFyZ*ee%AeXoZsEOqSne9z5A*>IzsP4vK=aY5EEh>+C&mR#p~%AG zAe=e;NvVDA3rykLOBXby3i9E$#a@WUJ3p@h)3)%-r1VA4*_YBthOy3hbu`%-B$k%F ze|<}XnQCG&vo^&{G?B|IiKdkf5rWT8P zFqQ1WTn96)G6NiGp?80ecR-MU?Dh>u`srpD>PV3wbvV*u`Z$_|U9e1&+;<5pVk&e?FJgPH!A84+oMnc+x@ zR^YIa7+5^83ZY^YfhkW1r@n@-jNdmzMP;|J_7Z*%1x zO*Vn9QyBk`wZd8BBBDKA`Cq>pj2)%896(UZJql=JsZ#Izn1o2YOQRa#k^<~GEIg}Z zqz?xkyfYu3T58q+fRMZ(I*m;M1IvD1bk} c7@hn60so#xc0k3w@Bjb+07*qoM6N<$f~Ap`ZU6uP literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html b/mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html new file mode 100644 index 0000000000..3afea1a0f2 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
<unnamed>43.2 %245 / 56771.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html b/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html new file mode 100644 index 0000000000..8077f5372c --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
<unnamed>43.2 %245 / 56771.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-detail.html b/mrs_uav_managers/src/control_manager/common/index-detail.html new file mode 100644 index 0000000000..8dbaf7b24a --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
<unnamed>43.2 %245 / 56771.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-sort-f.html b/mrs_uav_managers/src/control_manager/common/index-sort-f.html new file mode 100644 index 0000000000..98d1c0a8e1 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-sort-l.html b/mrs_uav_managers/src/control_manager/common/index-sort-l.html new file mode 100644 index 0000000000..23a1c8315c --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index.html b/mrs_uav_managers/src/control_manager/common/index.html new file mode 100644 index 0000000000..40beb19c9a --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24556743.2 %
Date:2024-04-25 05:42:57Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
43.2%43.2%
+ 		43.2 %245 / 56771.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..7894cdcf51 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html @@ -0,0 +1,524 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2379367064.8 %
Date:2024-04-25 05:42:57Functions:8411175.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetMinZ(mrs_msgs::Float64StampedSrvRequest_<std::allocator<void> >&, mrs_msgs::Float64StampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)2
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()4
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)6
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)8
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::elandSrv()8
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)12
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)16
mrs_uav_managers::control_manager::ControlManager::isOffboard()18
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)18
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)23
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)27
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)31
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)50
mrs_uav_managers::control_manager::ControlManager::ungripSrv()80
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)89
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::control_manager::ControlManager::initialize()94
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)94
mrs_uav_managers::control_manager::ControlManager::preinitialize()94
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)94
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)94
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)94
mrs_uav_managers::control_manager::ControlManager::onInit()94
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)95
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)97
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)104
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)116
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()149
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)157
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)184
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)185
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()198
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)216
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)219
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()260
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)325
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)372
mrs_uav_managers::control_manager::ControllerParams::ControllerParams(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, double, bool)470
mrs_uav_managers::control_manager::ControlManager::getMass()486
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)496
mrs_uav_managers::control_manager::TrackerParams::TrackerParams(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)565
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)631
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)720
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)720
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)770
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1630
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)2232
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9721
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)9789
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)9946
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)12245
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)12245
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)17162
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()17256
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()17522
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)22551
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)60884
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)70439
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)126575
mrs_uav_managers::control_manager::ControlManager::updateTrackers()127134
mrs_uav_managers::control_manager::ControlManager::asyncControl()136502
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)136855
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)136855
mrs_uav_managers::control_manager::ControlManager::publish()136855
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)158588
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)313812
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)404356
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.func.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.func.html new file mode 100644 index 0000000000..dcb11b90bd --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func.html @@ -0,0 +1,524 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2379367064.8 %
Date:2024-04-25 05:42:57Functions:8411175.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::control_manager::TrackerParams::TrackerParams(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)565
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)22551
mrs_uav_managers::control_manager::ControlManager::initialize()94
mrs_uav_managers::control_manager::ControlManager::isOffboard()18
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)496
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)2232
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)313812
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)17162
mrs_uav_managers::control_manager::ControlManager::asyncControl()136502
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)70439
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)27
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)94
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)50
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)116
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::preinitialize()94
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)216
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9721
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)60884
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)94
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::updateTrackers()127134
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetMinZ(mrs_msgs::Float64StampedSrvRequest_<std::allocator<void> >&, mrs_msgs::Float64StampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)8
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)158588
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()17522
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)219
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)104
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)136855
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)12
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()149
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()17256
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)404356
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)23
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)95
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)16
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)720
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)184
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)9946
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1630
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()260
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)97
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)6
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)157
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)94
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()198
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)185
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)325
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)94
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)9789
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)31
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)770
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)136855
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)372
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)720
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)126575
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)89
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)2
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)631
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)18
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()4
mrs_uav_managers::control_manager::ControlManager::onInit()94
mrs_uav_managers::control_manager::ControlManager::getMass()486
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)12245
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)12245
mrs_uav_managers::control_manager::ControlManager::publish()136855
mrs_uav_managers::control_manager::ControlManager::elandSrv()8
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::ungripSrv()80
mrs_uav_managers::control_manager::ControllerParams::ControllerParams(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, double, bool)470
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..34fe5178be --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html new file mode 100644 index 0000000000..91ab73b5c7 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html @@ -0,0 +1,8871 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2379367064.8 %
Date:2024-04-25 05:42:57Functions:8411175.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : #include <nodelet/nodelet.h>
+       6             : 
+       7             : #include <mrs_uav_managers/control_manager/common.h>
+       8             : #include <control_manager/output_publisher.h>
+       9             : 
+      10             : #include <mrs_uav_managers/controller.h>
+      11             : #include <mrs_uav_managers/tracker.h>
+      12             : 
+      13             : #include <mrs_msgs/String.h>
+      14             : #include <mrs_msgs/Float64Stamped.h>
+      15             : #include <mrs_msgs/Float64StampedSrv.h>
+      16             : #include <mrs_msgs/ObstacleSectors.h>
+      17             : #include <mrs_msgs/BoolStamped.h>
+      18             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      19             : #include <mrs_msgs/DynamicsConstraints.h>
+      20             : #include <mrs_msgs/ControlError.h>
+      21             : #include <mrs_msgs/GetFloat64.h>
+      22             : #include <mrs_msgs/ValidateReference.h>
+      23             : #include <mrs_msgs/ValidateReferenceList.h>
+      24             : #include <mrs_msgs/TrackerCommand.h>
+      25             : #include <mrs_msgs/EstimatorInput.h>
+      26             : 
+      27             : #include <geometry_msgs/Point32.h>
+      28             : #include <geometry_msgs/TwistStamped.h>
+      29             : #include <geometry_msgs/PoseArray.h>
+      30             : #include <geometry_msgs/Vector3Stamped.h>
+      31             : 
+      32             : #include <nav_msgs/Odometry.h>
+      33             : 
+      34             : #include <sensor_msgs/Joy.h>
+      35             : #include <sensor_msgs/NavSatFix.h>
+      36             : 
+      37             : #include <mrs_lib/safety_zone/safety_zone.h>
+      38             : #include <mrs_lib/profiler.h>
+      39             : #include <mrs_lib/param_loader.h>
+      40             : #include <mrs_lib/utils.h>
+      41             : #include <mrs_lib/mutex.h>
+      42             : #include <mrs_lib/transformer.h>
+      43             : #include <mrs_lib/geometry/misc.h>
+      44             : #include <mrs_lib/geometry/cyclic.h>
+      45             : #include <mrs_lib/attitude_converter.h>
+      46             : #include <mrs_lib/subscribe_handler.h>
+      47             : #include <mrs_lib/msg_extractor.h>
+      48             : #include <mrs_lib/quadratic_throttle_model.h>
+      49             : #include <mrs_lib/publisher_handler.h>
+      50             : #include <mrs_lib/service_client_handler.h>
+      51             : 
+      52             : #include <mrs_msgs/HwApiCapabilities.h>
+      53             : #include <mrs_msgs/HwApiStatus.h>
+      54             : #include <mrs_msgs/HwApiRcChannels.h>
+      55             : 
+      56             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      57             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      58             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      59             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      60             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      61             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      62             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      63             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      64             : #include <mrs_msgs/HwApiPositionCmd.h>
+      65             : 
+      66             : #include <std_msgs/Float64.h>
+      67             : 
+      68             : #include <future>
+      69             : 
+      70             : #include <pluginlib/class_loader.h>
+      71             : 
+      72             : #include <nodelet/loader.h>
+      73             : 
+      74             : #include <eigen3/Eigen/Eigen>
+      75             : 
+      76             : #include <visualization_msgs/Marker.h>
+      77             : #include <visualization_msgs/MarkerArray.h>
+      78             : 
+      79             : #include <mrs_msgs/Reference.h>
+      80             : #include <mrs_msgs/ReferenceStamped.h>
+      81             : #include <mrs_msgs/ReferenceList.h>
+      82             : #include <mrs_msgs/TrajectoryReference.h>
+      83             : 
+      84             : #include <mrs_msgs/ReferenceStampedSrv.h>
+      85             : #include <mrs_msgs/ReferenceStampedSrvRequest.h>
+      86             : #include <mrs_msgs/ReferenceStampedSrvResponse.h>
+      87             : 
+      88             : #include <mrs_msgs/VelocityReferenceStampedSrv.h>
+      89             : #include <mrs_msgs/VelocityReferenceStampedSrvRequest.h>
+      90             : #include <mrs_msgs/VelocityReferenceStampedSrvResponse.h>
+      91             : 
+      92             : #include <mrs_msgs/TransformReferenceSrv.h>
+      93             : #include <mrs_msgs/TransformReferenceSrvRequest.h>
+      94             : #include <mrs_msgs/TransformReferenceSrvResponse.h>
+      95             : 
+      96             : #include <mrs_msgs/TransformPoseSrv.h>
+      97             : #include <mrs_msgs/TransformPoseSrvRequest.h>
+      98             : #include <mrs_msgs/TransformPoseSrvResponse.h>
+      99             : 
+     100             : #include <mrs_msgs/TransformVector3Srv.h>
+     101             : #include <mrs_msgs/TransformVector3SrvRequest.h>
+     102             : #include <mrs_msgs/TransformVector3SrvResponse.h>
+     103             : 
+     104             : #include <mrs_msgs/Float64StampedSrv.h>
+     105             : #include <mrs_msgs/Float64StampedSrvRequest.h>
+     106             : #include <mrs_msgs/Float64StampedSrvResponse.h>
+     107             : 
+     108             : #include <mrs_msgs/Vec4.h>
+     109             : #include <mrs_msgs/Vec4Request.h>
+     110             : #include <mrs_msgs/Vec4Response.h>
+     111             : 
+     112             : #include <mrs_msgs/Vec1.h>
+     113             : #include <mrs_msgs/Vec1Request.h>
+     114             : #include <mrs_msgs/Vec1Response.h>
+     115             : 
+     116             : //}
+     117             : 
+     118             : /* defines //{ */
+     119             : 
+     120             : #define TAU 2 * M_PI
+     121             : #define REF_X 0
+     122             : #define REF_Y 1
+     123             : #define REF_Z 2
+     124             : #define REF_HEADING 3
+     125             : #define ELAND_STR "eland"
+     126             : #define EHOVER_STR "ehover"
+     127             : #define ESCALATING_FAILSAFE_STR "escalating_failsafe"
+     128             : #define FAILSAFE_STR "failsafe"
+     129             : #define INPUT_UAV_STATE 0
+     130             : #define INPUT_ODOMETRY 1
+     131             : #define RC_DEADBAND 0.2
+     132             : 
+     133             : //}
+     134             : 
+     135             : /* using //{ */
+     136             : 
+     137             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+     138             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+     139             : 
+     140             : using radians  = mrs_lib::geometry::radians;
+     141             : using sradians = mrs_lib::geometry::sradians;
+     142             : 
+     143             : //}
+     144             : 
+     145             : namespace mrs_uav_managers
+     146             : {
+     147             : 
+     148             : namespace control_manager
+     149             : {
+     150             : 
+     151             : /* //{ class ControlManager */
+     152             : 
+     153             : // state machine
+     154             : typedef enum
+     155             : {
+     156             : 
+     157             :   IDLE_STATE,
+     158             :   LANDING_STATE,
+     159             : 
+     160             : } LandingStates_t;
+     161             : 
+     162             : const char* state_names[2] = {"IDLING", "LANDING"};
+     163             : 
+     164             : // state machine
+     165             : typedef enum
+     166             : {
+     167             : 
+     168             :   FCU_FRAME,
+     169             :   RELATIVE_FRAME,
+     170             :   ABSOLUTE_FRAME
+     171             : 
+     172             : } ReferenceFrameType_t;
+     173             : 
+     174             : // state machine
+     175             : typedef enum
+     176             : {
+     177             : 
+     178             :   ESC_NONE_STATE     = 0,
+     179             :   ESC_EHOVER_STATE   = 1,
+     180             :   ESC_ELAND_STATE    = 2,
+     181             :   ESC_FAILSAFE_STATE = 3,
+     182             :   ESC_FINISHED_STATE = 4,
+     183             : 
+     184             : } EscalatingFailsafeStates_t;
+     185             : 
+     186             : /* class ControllerParams() //{ */
+     187             : 
+     188             : class ControllerParams {
+     189             : 
+     190             : public:
+     191             :   ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold, double odometry_innovation_threshold,
+     192             :                    bool human_switchable);
+     193             : 
+     194             : public:
+     195             :   double      failsafe_threshold;
+     196             :   double      eland_threshold;
+     197             :   double      odometry_innovation_threshold;
+     198             :   std::string address;
+     199             :   std::string name_space;
+     200             :   bool        human_switchable;
+     201             : };
+     202             : 
+     203         470 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+     204         470 :                                    double odometry_innovation_threshold, bool human_switchable) {
+     205             : 
+     206         470 :   this->eland_threshold               = eland_threshold;
+     207         470 :   this->odometry_innovation_threshold = odometry_innovation_threshold;
+     208         470 :   this->failsafe_threshold            = failsafe_threshold;
+     209         470 :   this->address                       = address;
+     210         470 :   this->name_space                    = name_space;
+     211         470 :   this->human_switchable              = human_switchable;
+     212         470 : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* class TrackerParams() //{ */
+     217             : 
+     218             : class TrackerParams {
+     219             : 
+     220             : public:
+     221             :   TrackerParams(std::string address, std::string name_space, bool human_switchable);
+     222             : 
+     223             : public:
+     224             :   std::string address;
+     225             :   std::string name_space;
+     226             :   bool        human_switchable;
+     227             : };
+     228             : 
+     229         565 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+     230             : 
+     231         565 :   this->address          = address;
+     232         565 :   this->name_space       = name_space;
+     233         565 :   this->human_switchable = human_switchable;
+     234         565 : }
+     235             : 
+     236             : //}
+     237             : 
+     238             : class ControlManager : public nodelet::Nodelet {
+     239             : 
+     240             : public:
+     241             :   virtual void onInit();
+     242             : 
+     243             : private:
+     244             :   ros::NodeHandle   nh_;
+     245             :   std::atomic<bool> is_initialized_ = false;
+     246             :   std::string       _uav_name_;
+     247             :   std::string       _body_frame_;
+     248             : 
+     249             :   std::string _custom_config_;
+     250             :   std::string _platform_config_;
+     251             :   std::string _world_config_;
+     252             :   std::string _network_config_;
+     253             : 
+     254             :   // | --------------- dynamic loading of trackers -------------- |
+     255             : 
+     256             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_;  // pluginlib loader of dynamically loaded trackers
+     257             :   std::vector<std::string>                                           _tracker_names_;  // list of tracker names
+     258             :   std::map<std::string, TrackerParams>                               trackers_;        // map between tracker names and tracker param
+     259             :   std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>>          tracker_list_;    // list of trackers, routines are callable from this
+     260             :   std::mutex                                                         mutex_tracker_list_;
+     261             : 
+     262             :   // | ------------- dynamic loading of controllers ------------- |
+     263             : 
+     264             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_;  // pluginlib loader of dynamically loaded controllers
+     265             :   std::vector<std::string>                                              _controller_names_;  // list of controller names
+     266             :   std::map<std::string, ControllerParams>                               controllers_;        // map between controller names and controller params
+     267             :   std::vector<boost::shared_ptr<mrs_uav_managers::Controller>>          controller_list_;    // list of controllers, routines are callable from this
+     268             :   std::mutex                                                            mutex_controller_list_;
+     269             : 
+     270             :   // | ------------------------- HW API ------------------------- |
+     271             : 
+     272             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     273             : 
+     274             :   OutputPublisher control_output_publisher_;
+     275             : 
+     276             :   ControlOutputModalities_t _hw_api_inputs_;
+     277             : 
+     278             :   double desired_uav_state_rate_ = 100.0;
+     279             : 
+     280             :   // this timer will check till we already got the hardware api diagnostics
+     281             :   // then it will trigger the initialization of the controllers and finish
+     282             :   // the initialization of the ControlManager
+     283             :   ros::Timer timer_hw_api_capabilities_;
+     284             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     285             : 
+     286             :   void preinitialize(void);
+     287             :   void initialize(void);
+     288             : 
+     289             :   // | ------------ tracker and controller switching ------------ |
+     290             : 
+     291             :   std::tuple<bool, std::string> switchController(const std::string& controller_name);
+     292             :   std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+     293             : 
+     294             :   // the time of last switching of a tracker or a controller
+     295             :   ros::Time  controller_tracker_switch_time_;
+     296             :   std::mutex mutex_controller_tracker_switch_time_;
+     297             : 
+     298             :   // | -------------------- the transformer  -------------------- |
+     299             : 
+     300             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     301             : 
+     302             :   // | ------------------- scope timer logger ------------------- |
+     303             : 
+     304             :   bool                                       scope_timer_enabled_ = false;
+     305             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     306             : 
+     307             :   // | --------------------- general params --------------------- |
+     308             : 
+     309             :   // defines the type of state input: odometry or uav_state mesasge types
+     310             :   int _state_input_;
+     311             : 
+     312             :   // names of important trackers
+     313             :   std::string _null_tracker_name_;     // null tracker is active when UAV is not in the air
+     314             :   std::string _ehover_tracker_name_;   // ehover tracker is used for emergency hovering
+     315             :   std::string _landoff_tracker_name_;  // landoff is used for landing and takeoff
+     316             : 
+     317             :   // names of important controllers
+     318             :   std::string _failsafe_controller_name_;  // controller used for feed-forward failsafe
+     319             :   std::string _eland_controller_name_;     // controller used for emergancy landing
+     320             : 
+     321             :   // joystick control
+     322             :   bool        _joystick_enabled_ = false;
+     323             :   int         _joystick_mode_;
+     324             :   std::string _joystick_tracker_name_;
+     325             :   std::string _joystick_controller_name_;
+     326             :   std::string _joystick_fallback_tracker_name_;
+     327             :   std::string _joystick_fallback_controller_name_;
+     328             : 
+     329             :   // should disarm after emergancy landing?
+     330             :   bool _eland_disarm_enabled_ = false;
+     331             : 
+     332             :   // enabling the emergency handoff -> will disable eland and failsafe
+     333             :   bool _rc_emergency_handoff_ = false;
+     334             : 
+     335             :   // what throttle should be output when null tracker is active?
+     336             :   double _min_throttle_null_tracker_ = 0.0;
+     337             : 
+     338             :   // rates of all the timers
+     339             :   double _status_timer_rate_   = 0;
+     340             :   double _safety_timer_rate_   = 0;
+     341             :   double _elanding_timer_rate_ = 0;
+     342             :   double _failsafe_timer_rate_ = 0;
+     343             :   double _bumper_timer_rate_   = 0;
+     344             : 
+     345             :   bool _snap_trajectory_to_safety_area_ = false;
+     346             : 
+     347             :   // | -------------- uav_state/odometry subscriber ------------- |
+     348             : 
+     349             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+     350             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     351             : 
+     352             :   mrs_msgs::UavState uav_state_;
+     353             :   mrs_msgs::UavState previous_uav_state_;
+     354             :   bool               got_uav_state_               = false;
+     355             :   double             _uav_state_max_missing_time_ = 0;  // how long should we tolerate missing state estimate?
+     356             :   double             uav_roll_                    = 0;
+     357             :   double             uav_pitch_                   = 0;
+     358             :   double             uav_yaw_                     = 0;
+     359             :   double             uav_heading_                 = 0;
+     360             :   std::mutex         mutex_uav_state_;
+     361             : 
+     362             :   // odometry hiccup detection
+     363             :   double uav_state_avg_dt_        = 1;
+     364             :   double uav_state_hiccup_factor_ = 1;
+     365             :   int    uav_state_count_         = 0;
+     366             : 
+     367             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     368             : 
+     369             :   // | -------------- safety area max z subscriber -------------- |
+     370             : 
+     371             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+     372             : 
+     373             :   // | ------------- odometry innovation subscriber ------------- |
+     374             : 
+     375             :   // odometry innovation is published by the odometry node
+     376             :   // it is used to issue eland if the estimator's input is too wonky
+     377             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+     378             : 
+     379             :   // | --------------------- common handlers -------------------- |
+     380             : 
+     381             :   // contains handlers that are shared with trackers and controllers
+     382             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+     383             : 
+     384             :   // | --------------- tracker and controller IDs --------------- |
+     385             : 
+     386             :   // keeping track of currently active controllers and trackers
+     387             :   unsigned int active_tracker_idx_    = 0;
+     388             :   unsigned int active_controller_idx_ = 0;
+     389             : 
+     390             :   // indeces of some notable trackers
+     391             :   unsigned int _ehover_tracker_idx_               = 0;
+     392             :   unsigned int _landoff_tracker_idx_              = 0;
+     393             :   unsigned int _joystick_tracker_idx_             = 0;
+     394             :   unsigned int _joystick_controller_idx_          = 0;
+     395             :   unsigned int _failsafe_controller_idx_          = 0;
+     396             :   unsigned int _joystick_fallback_controller_idx_ = 0;
+     397             :   unsigned int _joystick_fallback_tracker_idx_    = 0;
+     398             :   unsigned int _null_tracker_idx_                 = 0;
+     399             :   unsigned int _eland_controller_idx_             = 0;
+     400             : 
+     401             :   // | -------------- enabling the output publisher ------------- |
+     402             : 
+     403             :   void              toggleOutput(const bool& input);
+     404             :   std::atomic<bool> output_enabled_ = false;
+     405             : 
+     406             :   // | ----------------------- publishers ----------------------- |
+     407             : 
+     408             :   mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>     ph_controller_diagnostics_;
+     409             :   mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>            ph_tracker_cmd_;
+     410             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>            ph_mrs_odom_input_;
+     411             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>                  ph_control_reference_odom_;
+     412             :   mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+     413             :   mrs_lib::PublisherHandler<std_msgs::Empty>                     ph_offboard_on_;
+     414             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_tilt_error_;
+     415             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_estimate_;
+     416             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_nominal_;
+     417             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_throttle_;
+     418             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_thrust_;
+     419             :   mrs_lib::PublisherHandler<mrs_msgs::ControlError>              ph_control_error_;
+     420             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_markers_;
+     421             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_coordinates_markers_;
+     422             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_disturbances_markers_;
+     423             :   mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>       ph_current_constraints_;
+     424             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_heading_;
+     425             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_speed_;
+     426             : 
+     427             :   // | --------------------- service servers -------------------- |
+     428             : 
+     429             :   ros::ServiceServer service_server_switch_tracker_;
+     430             :   ros::ServiceServer service_server_switch_controller_;
+     431             :   ros::ServiceServer service_server_reset_tracker_;
+     432             :   ros::ServiceServer service_server_hover_;
+     433             :   ros::ServiceServer service_server_ehover_;
+     434             :   ros::ServiceServer service_server_failsafe_;
+     435             :   ros::ServiceServer service_server_failsafe_escalating_;
+     436             :   ros::ServiceServer service_server_toggle_output_;
+     437             :   ros::ServiceServer service_server_arm_;
+     438             :   ros::ServiceServer service_server_enable_callbacks_;
+     439             :   ros::ServiceServer service_server_set_constraints_;
+     440             :   ros::ServiceServer service_server_use_joystick_;
+     441             :   ros::ServiceServer service_server_use_safety_area_;
+     442             :   ros::ServiceServer service_server_emergency_reference_;
+     443             :   ros::ServiceServer service_server_pirouette_;
+     444             :   ros::ServiceServer service_server_eland_;
+     445             :   ros::ServiceServer service_server_parachute_;
+     446             :   ros::ServiceServer service_server_set_min_z_;
+     447             : 
+     448             :   // human callbable services for references
+     449             :   ros::ServiceServer service_server_goto_;
+     450             :   ros::ServiceServer service_server_goto_fcu_;
+     451             :   ros::ServiceServer service_server_goto_relative_;
+     452             :   ros::ServiceServer service_server_goto_altitude_;
+     453             :   ros::ServiceServer service_server_set_heading_;
+     454             :   ros::ServiceServer service_server_set_heading_relative_;
+     455             : 
+     456             :   // the reference service and subscriber
+     457             :   ros::ServiceServer                                    service_server_reference_;
+     458             :   mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+     459             : 
+     460             :   // the velocity reference service and subscriber
+     461             :   ros::ServiceServer                                            service_server_velocity_reference_;
+     462             :   mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+     463             : 
+     464             :   // trajectory tracking
+     465             :   ros::ServiceServer                                       service_server_trajectory_reference_;
+     466             :   mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+     467             :   ros::ServiceServer                                       service_server_start_trajectory_tracking_;
+     468             :   ros::ServiceServer                                       service_server_stop_trajectory_tracking_;
+     469             :   ros::ServiceServer                                       service_server_resume_trajectory_tracking_;
+     470             :   ros::ServiceServer                                       service_server_goto_trajectory_start_;
+     471             : 
+     472             :   // transform service servers
+     473             :   ros::ServiceServer service_server_transform_reference_;
+     474             :   ros::ServiceServer service_server_transform_pose_;
+     475             :   ros::ServiceServer service_server_transform_vector3_;
+     476             : 
+     477             :   // safety area services
+     478             :   ros::ServiceServer service_server_validate_reference_;
+     479             :   ros::ServiceServer service_server_validate_reference_2d_;
+     480             :   ros::ServiceServer service_server_validate_reference_list_;
+     481             : 
+     482             :   // bumper service servers
+     483             :   ros::ServiceServer service_server_bumper_enabler_;
+     484             :   ros::ServiceServer service_server_bumper_repulsion_enabler_;
+     485             : 
+     486             :   // service clients
+     487             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+     488             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+     489             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+     490             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+     491             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+     492             : 
+     493             :   // safety area min z servers
+     494             :   ros::ServiceServer service_server_get_min_z_;
+     495             : 
+     496             :   // | --------- trackers' and controllers' last results -------- |
+     497             : 
+     498             :   // the last result of an active tracker
+     499             :   std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+     500             :   std::mutex                              mutex_last_tracker_cmd_;
+     501             : 
+     502             :   // the last result of an active controller
+     503             :   Controller::ControlOutput last_control_output_;
+     504             :   std::mutex                mutex_last_control_output_;
+     505             : 
+     506             :   // | -------------- HW API diagnostics subscriber ------------- |
+     507             : 
+     508             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+     509             : 
+     510             :   bool offboard_mode_          = false;
+     511             :   bool offboard_mode_was_true_ = false;  // if it was ever true
+     512             :   bool armed_                  = false;
+     513             : 
+     514             :   // | -------------------- throttle and mass ------------------- |
+     515             : 
+     516             :   // throttle mass estimation during eland
+     517             :   double    throttle_mass_estimate_   = 0;
+     518             :   bool      throttle_under_threshold_ = false;
+     519             :   ros::Time throttle_mass_estimate_first_time_;
+     520             : 
+     521             :   // | ---------------------- safety params --------------------- |
+     522             : 
+     523             :   // failsafe when tilt error is too large
+     524             :   bool   _tilt_error_disarm_enabled_;
+     525             :   double _tilt_error_disarm_timeout_;
+     526             :   double _tilt_error_disarm_threshold_;
+     527             : 
+     528             :   ros::Time tilt_error_disarm_time_;
+     529             :   bool      tilt_error_disarm_over_thr_ = false;
+     530             : 
+     531             :   // elanding when tilt error is too large
+     532             :   bool   _tilt_limit_eland_enabled_;
+     533             :   double _tilt_limit_eland_ = 0;  // [rad]
+     534             : 
+     535             :   // disarming when tilt error is too large
+     536             :   bool   _tilt_limit_disarm_enabled_;
+     537             :   double _tilt_limit_disarm_ = 0;  // [rad]
+     538             : 
+     539             :   // elanding when yaw error is too large
+     540             :   bool   _yaw_error_eland_enabled_;
+     541             :   double _yaw_error_eland_ = 0;  // [rad]
+     542             : 
+     543             :   // keeping track of control errors
+     544             :   std::optional<double> tilt_error_ = 0;
+     545             :   std::optional<double> yaw_error_  = 0;
+     546             :   std::mutex            mutex_attitude_error_;
+     547             : 
+     548             :   std::optional<Eigen::Vector3d> position_error_;
+     549             :   std::mutex                     mutex_position_error_;
+     550             : 
+     551             :   // control error for triggering failsafe, eland, etc.
+     552             :   // this filled with the current controllers failsafe threshold
+     553             :   double _failsafe_threshold_                = 0;  // control error for triggering failsafe
+     554             :   double _eland_threshold_                   = 0;  // control error for triggering eland
+     555             :   bool   _odometry_innovation_check_enabled_ = false;
+     556             :   double _odometry_innovation_threshold_     = 0;  // innovation size for triggering eland
+     557             : 
+     558             :   bool callbacks_enabled_ = true;
+     559             : 
+     560             :   // | ------------------------ parachute ----------------------- |
+     561             : 
+     562             :   bool _parachute_enabled_ = false;
+     563             : 
+     564             :   std::tuple<bool, std::string> deployParachute(void);
+     565             :   bool                          parachuteSrv(void);
+     566             : 
+     567             :   // | ----------------------- safety area ---------------------- |
+     568             : 
+     569             :   // safety area
+     570             :   std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+     571             : 
+     572             :   std::atomic<bool> use_safety_area_ = false;
+     573             : 
+     574             :   std::string _safety_area_horizontal_frame_;
+     575             :   std::string _safety_area_vertical_frame_;
+     576             : 
+     577             :   std::atomic<double> _safety_area_min_z_ = 0;
+     578             : 
+     579             :   double _safety_area_max_z_ = 0;
+     580             : 
+     581             :   // safety area routines
+     582             :   // those are passed to trackers using the common_handlers object
+     583             :   bool   isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+     584             :   bool   isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+     585             :   bool   isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     586             :   bool   isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     587             :   double getMinZ(const std::string& frame_id);
+     588             :   double getMaxZ(const std::string& frame_id);
+     589             : 
+     590             :   // | ------------------------ callbacks ----------------------- |
+     591             : 
+     592             :   // topic callbacks
+     593             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     594             :   void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     595             :   void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     596             :   void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     597             :   void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+     598             : 
+     599             :   // topic timeouts
+     600             :   void timeoutUavState(const double& missing_for);
+     601             : 
+     602             :   // switching controller and tracker services
+     603             :   bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     604             :   bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     605             :   bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     606             : 
+     607             :   // reference callbacks
+     608             :   void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+     609             :   void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+     610             :   void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+     611             :   bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     612             :   bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     613             :   bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     614             :   bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     615             :   bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     616             :   bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     617             :   bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     618             :   bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+     619             :   bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+     620             :   bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     621             : 
+     622             :   // safety callbacks
+     623             :   bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     624             :   bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     625             :   bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     626             :   bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     627             :   bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     628             :   bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     629             :   bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     630             :   bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     631             :   bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     632             :   bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     633             :   bool callbackSetMinZ(mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res);
+     634             :   bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     635             :   bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     636             :   bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     637             :   bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     638             :   bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     639             : 
+     640             :   bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+     641             : 
+     642             :   bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     643             :   bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     644             :   bool callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res);
+     645             : 
+     646             :   // transformation callbacks
+     647             :   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+     648             :   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+     649             :   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+     650             : 
+     651             :   // | ----------------------- constraints ---------------------- |
+     652             : 
+     653             :   // sets constraints to all trackers
+     654             :   bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+     655             : 
+     656             :   // constraints management
+     657             :   bool              got_constraints_ = false;
+     658             :   std::mutex        mutex_constraints_;
+     659             :   void              setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     660             :   void              setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     661             :   void              setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     662             :   std::atomic<bool> constraints_being_enforced_ = false;
+     663             : 
+     664             :   std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     665             : 
+     666             :   mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+     667             :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+     668             : 
+     669             :   // | ------------------ emergency triggered? ------------------ |
+     670             : 
+     671             :   std::atomic<bool> failsafe_triggered_ = false;
+     672             :   std::atomic<bool> eland_triggered_    = false;
+     673             : 
+     674             :   // | ------------------------- timers ------------------------- |
+     675             : 
+     676             :   // timer for regular status publishing
+     677             :   ros::Timer timer_status_;
+     678             :   void       timerStatus(const ros::TimerEvent& event);
+     679             : 
+     680             :   // timer for issuing the failsafe landing
+     681             :   ros::Timer timer_failsafe_;
+     682             :   void       timerFailsafe(const ros::TimerEvent& event);
+     683             : 
+     684             :   // oneshot timer for running controllers and trackers
+     685             :   void              asyncControl(void);
+     686             :   std::atomic<bool> running_async_control_ = false;
+     687             :   std::future<void> async_control_result_;
+     688             : 
+     689             :   // timer for issuing emergancy landing
+     690             :   ros::Timer timer_eland_;
+     691             :   void       timerEland(const ros::TimerEvent& event);
+     692             : 
+     693             :   // timer for regular checking of controller errors
+     694             :   ros::Timer        timer_safety_;
+     695             :   void              timerSafety(const ros::TimerEvent& event);
+     696             :   std::atomic<bool> running_safety_timer_        = false;
+     697             :   std::atomic<bool> odometry_switch_in_progress_ = false;
+     698             : 
+     699             :   // timer for issuing the pirouette
+     700             :   ros::Timer timer_pirouette_;
+     701             :   void       timerPirouette(const ros::TimerEvent& event);
+     702             : 
+     703             :   // | --------------------- obstacle bumper -------------------- |
+     704             : 
+     705             :   // bumper timer
+     706             :   ros::Timer timer_bumper_;
+     707             :   void       timerBumper(const ros::TimerEvent& event);
+     708             : 
+     709             :   // bumper subscriber
+     710             :   mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+     711             : 
+     712             :   bool        _bumper_switch_tracker_    = false;
+     713             :   bool        _bumper_switch_controller_ = false;
+     714             :   std::string _bumper_tracker_name_;
+     715             :   std::string _bumper_controller_name_;
+     716             :   std::string bumper_previous_tracker_;
+     717             :   std::string bumper_previous_controller_;
+     718             : 
+     719             :   std::atomic<bool> bumper_enabled_   = false;
+     720             :   std::atomic<bool> bumper_repulsing_ = false;
+     721             : 
+     722             :   bool _bumper_horizontal_derive_from_dynamics_;
+     723             :   bool _bumper_vertical_derive_from_dynamics_;
+     724             : 
+     725             :   double _bumper_horizontal_distance_ = 0;
+     726             :   double _bumper_vertical_distance_   = 0;
+     727             : 
+     728             :   double _bumper_horizontal_overshoot_ = 0;
+     729             :   double _bumper_vertical_overshoot_   = 0;
+     730             : 
+     731             :   int  bumperGetSectorId(const double& x, const double& y, const double& z);
+     732             :   void bumperPushFromObstacle(void);
+     733             : 
+     734             :   // | --------------- safety checks and failsafes -------------- |
+     735             : 
+     736             :   // escalating failsafe (eland -> failsafe -> disarm)
+     737             :   bool                       _service_escalating_failsafe_enabled_ = false;
+     738             :   bool                       _rc_escalating_failsafe_enabled_      = false;
+     739             :   double                     _escalating_failsafe_timeout_         = 0;
+     740             :   ros::Time                  escalating_failsafe_time_;
+     741             :   bool                       _escalating_failsafe_ehover_   = false;
+     742             :   bool                       _escalating_failsafe_eland_    = false;
+     743             :   bool                       _escalating_failsafe_failsafe_ = false;
+     744             :   double                     _rc_escalating_failsafe_threshold_;
+     745             :   int                        _rc_escalating_failsafe_channel_  = 0;
+     746             :   bool                       rc_escalating_failsafe_triggered_ = false;
+     747             :   EscalatingFailsafeStates_t state_escalating_failsafe_        = ESC_NONE_STATE;
+     748             : 
+     749             :   std::string _tracker_error_action_;
+     750             : 
+     751             :   // emergancy landing state machine
+     752             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     753             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     754             :   std::mutex      mutex_landing_state_machine_;
+     755             :   void            changeLandingState(LandingStates_t new_state);
+     756             :   double          _uav_mass_ = 0;
+     757             :   double          _elanding_cutoff_mass_factor_;
+     758             :   double          _elanding_cutoff_timeout_;
+     759             :   double          landing_uav_mass_ = 0;
+     760             : 
+     761             :   // initial body disturbance loaded from params
+     762             :   double _initial_body_disturbance_x_ = 0;
+     763             :   double _initial_body_disturbance_y_ = 0;
+     764             : 
+     765             :   // profiling
+     766             :   mrs_lib::Profiler profiler_;
+     767             :   bool              _profiler_enabled_ = false;
+     768             : 
+     769             :   // diagnostics publishing
+     770             :   void       publishDiagnostics(void);
+     771             :   std::mutex mutex_diagnostics_;
+     772             : 
+     773             :   void                                             ungripSrv(void);
+     774             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+     775             : 
+     776             :   bool isFlyingNormally(void);
+     777             : 
+     778             :   // | ------------------------ pirouette ----------------------- |
+     779             : 
+     780             :   bool       _pirouette_enabled_ = false;
+     781             :   double     _pirouette_speed_;
+     782             :   double     _pirouette_timer_rate_;
+     783             :   std::mutex mutex_pirouette_;
+     784             :   double     pirouette_initial_heading_;
+     785             :   double     pirouette_iterator_;
+     786             :   bool       callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     787             : 
+     788             :   // | -------------------- joystick control -------------------- |
+     789             : 
+     790             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     791             : 
+     792             :   void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+     793             :   bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     794             : 
+     795             :   // joystick buttons mappings
+     796             :   int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+     797             : 
+     798             :   // channel numbers and channel multipliers
+     799             :   int    _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+     800             :   double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+     801             : 
+     802             :   ros::Timer timer_joystick_;
+     803             :   void       timerJoystick(const ros::TimerEvent& event);
+     804             :   double     _joystick_timer_rate_ = 0;
+     805             : 
+     806             :   double _joystick_carrot_distance_ = 0;
+     807             : 
+     808             :   ros::Time joystick_start_press_time_;
+     809             :   bool      joystick_start_pressed_ = false;
+     810             : 
+     811             :   ros::Time joystick_back_press_time_;
+     812             :   bool      joystick_back_pressed_ = false;
+     813             :   bool      joystick_goto_enabled_ = false;
+     814             : 
+     815             :   bool      joystick_failsafe_pressed_ = false;
+     816             :   ros::Time joystick_failsafe_press_time_;
+     817             : 
+     818             :   bool      joystick_eland_pressed_ = false;
+     819             :   ros::Time joystick_eland_press_time_;
+     820             : 
+     821             :   // | ------------------- RC joystick control ------------------ |
+     822             : 
+     823             :   // listening to the RC channels as told by pixhawk
+     824             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+     825             : 
+     826             :   // the RC channel mapping of the main 4 control signals
+     827             :   double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+     828             : 
+     829             :   bool              _rc_goto_enabled_               = false;
+     830             :   std::atomic<bool> rc_goto_active_                 = false;
+     831             :   double            rc_joystick_channel_last_value_ = 0.5;
+     832             :   bool              rc_joystick_channel_was_low_    = false;
+     833             :   int               _rc_joystick_channel_           = 0;
+     834             : 
+     835             :   double _rc_horizontal_speed_ = 0;
+     836             :   double _rc_vertical_speed_   = 0;
+     837             :   double _rc_heading_rate_     = 0;
+     838             : 
+     839             :   // | ------------------- trajectory loading ------------------- |
+     840             : 
+     841             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_original_trajectory_poses_;
+     842             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+     843             : 
+     844             :   // | --------------------- other routines --------------------- |
+     845             : 
+     846             :   // this is called to update the trackers and to receive position control command from the active one
+     847             :   void updateTrackers(void);
+     848             : 
+     849             :   // this is called to update the controllers and to receive attitude control command from the active one
+     850             :   void updateControllers(const mrs_msgs::UavState& uav_state);
+     851             : 
+     852             :   // sets the reference to the active tracker
+     853             :   std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+     854             : 
+     855             :   // sets the velocity reference to the active tracker
+     856             :   std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+     857             : 
+     858             :   // sets the reference trajectory to the active tracker
+     859             :   std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+     860             :       const mrs_msgs::TrajectoryReference trajectory_in);
+     861             : 
+     862             :   // publishes
+     863             :   void publish(void);
+     864             : 
+     865             :   bool loadConfigFile(const std::string& file_path, const std::string ns);
+     866             : 
+     867             :   double getMass(void);
+     868             : 
+     869             :   // publishes rviz-visualizable control reference
+     870             :   void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+     871             : 
+     872             :   void initializeControlOutput(void);
+     873             : 
+     874             :   // tell the mrs_odometry to disable its callbacks
+     875             :   void odometryCallbacksSrv(const bool input);
+     876             : 
+     877             :   mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+     878             : 
+     879             :   void                          setCallbacks(bool in);
+     880             :   bool                          isOffboard(void);
+     881             :   bool                          elandSrv(void);
+     882             :   std::tuple<bool, std::string> arming(const bool input);
+     883             : 
+     884             :   // safety functions impl
+     885             :   std::tuple<bool, std::string> ehover(void);
+     886             :   std::tuple<bool, std::string> hover(void);
+     887             :   std::tuple<bool, std::string> startTrajectoryTracking(void);
+     888             :   std::tuple<bool, std::string> stopTrajectoryTracking(void);
+     889             :   std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+     890             :   std::tuple<bool, std::string> gotoTrajectoryStart(void);
+     891             :   std::tuple<bool, std::string> eland(void);
+     892             :   std::tuple<bool, std::string> failsafe(void);
+     893             :   std::tuple<bool, std::string> escalatingFailsafe(void);
+     894             : 
+     895             :   EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+     896             : };
+     897             : 
+     898             : //}
+     899             : 
+     900             : /* //{ onInit() */
+     901             : 
+     902          94 : void ControlManager::onInit() {
+     903          94 :   preinitialize();
+     904          94 : }
+     905             : 
+     906             : //}
+     907             : 
+     908             : /* preinitialize() //{ */
+     909             : 
+     910          94 : void ControlManager::preinitialize(void) {
+     911             : 
+     912          94 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     913             : 
+     914          94 :   ros::Time::waitForValid();
+     915             : 
+     916          94 :   mrs_lib::SubscribeHandlerOptions shopts;
+     917          94 :   shopts.nh                 = nh_;
+     918          94 :   shopts.node_name          = "ControlManager";
+     919          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     920          94 :   shopts.threadsafe         = true;
+     921          94 :   shopts.autostart          = true;
+     922          94 :   shopts.queue_size         = 10;
+     923          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     924             : 
+     925          94 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     926             : 
+     927          94 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+     928          94 : }
+     929             : 
+     930             : //}
+     931             : 
+     932             : /* initialize() //{ */
+     933             : 
+     934          94 : void ControlManager::initialize(void) {
+     935             : 
+     936          94 :   joystick_start_press_time_      = ros::Time(0);
+     937          94 :   joystick_failsafe_press_time_   = ros::Time(0);
+     938          94 :   joystick_eland_press_time_      = ros::Time(0);
+     939          94 :   escalating_failsafe_time_       = ros::Time(0);
+     940          94 :   controller_tracker_switch_time_ = ros::Time(0);
+     941             : 
+     942          94 :   ROS_INFO("[ControlManager]: initializing");
+     943             : 
+     944             :   // --------------------------------------------------------------
+     945             :   // |         common handler for trackers and controllers        |
+     946             :   // --------------------------------------------------------------
+     947             : 
+     948          94 :   common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+     949             : 
+     950             :   // --------------------------------------------------------------
+     951             :   // |                           params                           |
+     952             :   // --------------------------------------------------------------
+     953             : 
+     954         188 :   mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+     955             : 
+     956          94 :   param_loader.loadParam("custom_config", _custom_config_);
+     957          94 :   param_loader.loadParam("platform_config", _platform_config_);
+     958          94 :   param_loader.loadParam("world_config", _world_config_);
+     959          94 :   param_loader.loadParam("network_config", _network_config_);
+     960             : 
+     961          94 :   if (_custom_config_ != "") {
+     962          94 :     param_loader.addYamlFile(_custom_config_);
+     963             :   }
+     964             : 
+     965          94 :   if (_platform_config_ != "") {
+     966          94 :     param_loader.addYamlFile(_platform_config_);
+     967             :   }
+     968             : 
+     969          94 :   if (_world_config_ != "") {
+     970          94 :     param_loader.addYamlFile(_world_config_);
+     971             :   }
+     972             : 
+     973          94 :   if (_network_config_ != "") {
+     974          94 :     param_loader.addYamlFile(_network_config_);
+     975             :   }
+     976             : 
+     977          94 :   param_loader.addYamlFileFromParam("private_config");
+     978          94 :   param_loader.addYamlFileFromParam("public_config");
+     979          94 :   param_loader.addYamlFileFromParam("private_trackers");
+     980          94 :   param_loader.addYamlFileFromParam("private_controllers");
+     981          94 :   param_loader.addYamlFileFromParam("public_controllers");
+     982             : 
+     983             :   // params passed from the launch file are not prefixed
+     984          94 :   param_loader.loadParam("uav_name", _uav_name_);
+     985          94 :   param_loader.loadParam("body_frame", _body_frame_);
+     986          94 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     987          94 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     988          94 :   param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+     989          94 :   param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+     990          94 :   param_loader.loadParam("g", common_handlers_->g);
+     991             : 
+     992             :   // motor params are also not prefixed, since they are common to more nodes
+     993          94 :   param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+     994          94 :   param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+     995          94 :   param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+     996             : 
+     997             :   // | ----------------------- safety area ---------------------- |
+     998             : 
+     999             :   bool use_safety_area;
+    1000          94 :   param_loader.loadParam("safety_area/enabled", use_safety_area);
+    1001          94 :   use_safety_area_ = use_safety_area;
+    1002             : 
+    1003          94 :   param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+    1004             : 
+    1005          94 :   param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+    1006          94 :   param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+    1007             : 
+    1008             :   {
+    1009             :     double temp;
+    1010          94 :     param_loader.loadParam("safety_area/vertical/min_z", temp);
+    1011             : 
+    1012          94 :     _safety_area_min_z_ = temp;
+    1013             :   }
+    1014             : 
+    1015          94 :   if (use_safety_area_) {
+    1016             : 
+    1017         219 :     Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+    1018             : 
+    1019             :     try {
+    1020             : 
+    1021         146 :       std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+    1022          73 :       std::vector<Eigen::MatrixXd> point_obstacle_points;
+    1023             : 
+    1024          73 :       safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+    1025             :     }
+    1026             : 
+    1027           0 :     catch (mrs_lib::SafetyZone::BorderError& e) {
+    1028           0 :       ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+    1029           0 :       ros::shutdown();
+    1030             :     }
+    1031           0 :     catch (...) {
+    1032           0 :       ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+    1033           0 :       ros::shutdown();
+    1034             :     }
+    1035             : 
+    1036          73 :     ROS_INFO("[ControlManager]: safety area initialized");
+    1037             :   }
+    1038             : 
+    1039          94 :   param_loader.setPrefix("mrs_uav_managers/control_manager/");
+    1040             : 
+    1041          94 :   param_loader.loadParam("state_input", _state_input_);
+    1042             : 
+    1043          94 :   if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+    1044           0 :     ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+    1045           0 :     ros::shutdown();
+    1046             :   }
+    1047             : 
+    1048          94 :   param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+    1049          94 :   param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+    1050          94 :   param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+    1051             : 
+    1052          94 :   param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+    1053          94 :   param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+    1054          94 :   param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+    1055          94 :   param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+    1056          94 :   param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+    1057             : 
+    1058          94 :   param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+    1059          94 :   param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+    1060          94 :   param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+    1061          94 :   param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+    1062          94 :   param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+    1063          94 :   param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+    1064          94 :   param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+    1065          94 :   param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+    1066             : 
+    1067          94 :   param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+    1068          94 :   param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+    1069             : 
+    1070          94 :   _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+    1071             : 
+    1072          94 :   if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+    1073           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+    1074           0 :     ros::shutdown();
+    1075             :   }
+    1076             : 
+    1077          94 :   param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+    1078          94 :   param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+    1079             : 
+    1080          94 :   _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+    1081             : 
+    1082          94 :   if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+    1083           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+    1084           0 :     ros::shutdown();
+    1085             :   }
+    1086             : 
+    1087          94 :   param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+    1088          94 :   param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+    1089             : 
+    1090          94 :   _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+    1091             : 
+    1092          94 :   if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+    1093           0 :     ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+    1094           0 :     ros::shutdown();
+    1095             :   }
+    1096             : 
+    1097          94 :   param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+    1098          94 :   param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+    1099          94 :   param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+    1100          94 :   param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+    1101             : 
+    1102          94 :   param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+    1103          94 :   param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+    1104             : 
+    1105          94 :   param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+    1106          94 :   param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+    1107          94 :   param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+    1108             : 
+    1109          94 :   _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+    1110             : 
+    1111          94 :   if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+    1112           0 :     ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+    1113           0 :     ros::shutdown();
+    1114             :   }
+    1115             : 
+    1116             :   // default constraints
+    1117             : 
+    1118          94 :   param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+    1119          94 :   param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+    1120          94 :   param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+    1121          94 :   param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+    1122             : 
+    1123          94 :   param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+    1124          94 :   param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+    1125          94 :   param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+    1126          94 :   param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+    1127             : 
+    1128          94 :   param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+    1129          94 :   param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+    1130          94 :   param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+    1131          94 :   param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+    1132             : 
+    1133          94 :   param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+    1134          94 :   param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+    1135          94 :   param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+    1136          94 :   param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+    1137             : 
+    1138          94 :   param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+    1139          94 :   param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+    1140          94 :   param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+    1141             : 
+    1142          94 :   param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+    1143             : 
+    1144          94 :   current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+    1145             : 
+    1146             :   // joystick
+    1147             : 
+    1148          94 :   param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+    1149          94 :   param_loader.loadParam("joystick/mode", _joystick_mode_);
+    1150          94 :   param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+    1151          94 :   param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+    1152          94 :   param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+    1153          94 :   param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+    1154          94 :   param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+    1155          94 :   param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+    1156             : 
+    1157          94 :   param_loader.loadParam("joystick/channels/A", _channel_A_);
+    1158          94 :   param_loader.loadParam("joystick/channels/B", _channel_B_);
+    1159          94 :   param_loader.loadParam("joystick/channels/X", _channel_X_);
+    1160          94 :   param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+    1161          94 :   param_loader.loadParam("joystick/channels/start", _channel_start_);
+    1162          94 :   param_loader.loadParam("joystick/channels/back", _channel_back_);
+    1163          94 :   param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+    1164          94 :   param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+    1165          94 :   param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+    1166          94 :   param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+    1167             : 
+    1168             :   // load channels
+    1169          94 :   param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+    1170          94 :   param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+    1171          94 :   param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+    1172          94 :   param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+    1173             : 
+    1174             :   // load channel multipliers
+    1175          94 :   param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+    1176          94 :   param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+    1177          94 :   param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+    1178          94 :   param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+    1179             : 
+    1180             :   bool bumper_enabled;
+    1181          94 :   param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+    1182          94 :   bumper_enabled_ = bumper_enabled;
+    1183             : 
+    1184          94 :   param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+    1185          94 :   param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+    1186          94 :   param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+    1187          94 :   param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+    1188          94 :   param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+    1189             : 
+    1190          94 :   param_loader.loadParam("obstacle_bumper/horizontal/min_distance_to_obstacle", _bumper_horizontal_distance_);
+    1191          94 :   param_loader.loadParam("obstacle_bumper/horizontal/derived_from_dynamics", _bumper_horizontal_derive_from_dynamics_);
+    1192             : 
+    1193          94 :   param_loader.loadParam("obstacle_bumper/vertical/min_distance_to_obstacle", _bumper_vertical_distance_);
+    1194          94 :   param_loader.loadParam("obstacle_bumper/vertical/derived_from_dynamics", _bumper_vertical_derive_from_dynamics_);
+    1195             : 
+    1196          94 :   param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+    1197          94 :   param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+    1198             : 
+    1199          94 :   param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+    1200             : 
+    1201          94 :   param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+    1202             : 
+    1203             :   // check the values of tracker error action
+    1204          94 :   if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+    1205           0 :     ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+    1206             :               EHOVER_STR);
+    1207           0 :     ros::shutdown();
+    1208             :   }
+    1209             : 
+    1210          94 :   param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+    1211          94 :   param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+    1212          94 :   param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+    1213          94 :   param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+    1214          94 :   param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+    1215             : 
+    1216          94 :   param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+    1217          94 :   param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+    1218          94 :   param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+    1219          94 :   param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+    1220             : 
+    1221          94 :   param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+    1222          94 :   param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+    1223             : 
+    1224          94 :   param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+    1225             : 
+    1226             :   // --------------------------------------------------------------
+    1227             :   // |             initialize the last control output             |
+    1228             :   // --------------------------------------------------------------
+    1229             : 
+    1230          94 :   initializeControlOutput();
+    1231             : 
+    1232             :   // | --------------------- tf transformer --------------------- |
+    1233             : 
+    1234          94 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+    1235          94 :   transformer_->setDefaultPrefix(_uav_name_);
+    1236          94 :   transformer_->retryLookupNewest(true);
+    1237             : 
+    1238             :   // | ------------------- scope timer logger ------------------- |
+    1239             : 
+    1240          94 :   param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+    1241         282 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+    1242          94 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+    1243             : 
+    1244             :   // bind transformer to trackers and controllers for use
+    1245          94 :   common_handlers_->transformer = transformer_;
+    1246             : 
+    1247             :   // bind scope timer to trackers and controllers for use
+    1248          94 :   common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+    1249          94 :   common_handlers_->scope_timer.logger  = scope_timer_logger_;
+    1250             : 
+    1251          94 :   common_handlers_->safety_area.use_safety_area       = use_safety_area_;
+    1252          94 :   common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+    1253          94 :   common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+    1254          94 :   common_handlers_->safety_area.getMinZ               = boost::bind(&ControlManager::getMinZ, this, _1);
+    1255          94 :   common_handlers_->safety_area.getMaxZ               = boost::bind(&ControlManager::getMaxZ, this, _1);
+    1256             : 
+    1257          94 :   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+    1258             : 
+    1259          94 :   common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+    1260             : 
+    1261          94 :   common_handlers_->control_output_modalities = _hw_api_inputs_;
+    1262             : 
+    1263          94 :   common_handlers_->uav_name = _uav_name_;
+    1264             : 
+    1265          94 :   common_handlers_->parent_nh = nh_;
+    1266             : 
+    1267             :   // --------------------------------------------------------------
+    1268             :   // |                        load trackers                       |
+    1269             :   // --------------------------------------------------------------
+    1270             : 
+    1271         188 :   std::vector<std::string> custom_trackers;
+    1272             : 
+    1273          94 :   param_loader.loadParam("mrs_trackers", _tracker_names_);
+    1274          94 :   param_loader.loadParam("trackers", custom_trackers);
+    1275             : 
+    1276          94 :   if (!custom_trackers.empty()) {
+    1277           1 :     _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+    1278             :   }
+    1279             : 
+    1280          94 :   param_loader.loadParam("null_tracker", _null_tracker_name_);
+    1281          94 :   param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+    1282             : 
+    1283          94 :   tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+    1284             : 
+    1285         659 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    1286             : 
+    1287        1130 :     std::string tracker_name = _tracker_names_.at(i);
+    1288             : 
+    1289             :     // load the controller parameters
+    1290        1130 :     std::string address;
+    1291        1130 :     std::string name_space;
+    1292             :     bool        human_switchable;
+    1293             : 
+    1294         565 :     param_loader.loadParam(tracker_name + "/address", address);
+    1295         565 :     param_loader.loadParam(tracker_name + "/namespace", name_space);
+    1296         565 :     param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+    1297             : 
+    1298        1695 :     TrackerParams new_tracker(address, name_space, human_switchable);
+    1299         565 :     trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+    1300             : 
+    1301             :     try {
+    1302         565 :       ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+    1303         565 :       tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+    1304             :     }
+    1305           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1306           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+    1307           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1308           0 :       ros::shutdown();
+    1309             :     }
+    1310           0 :     catch (pluginlib::PluginlibException& ex) {
+    1311           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+    1312           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1313           0 :       ros::shutdown();
+    1314             :     }
+    1315             :   }
+    1316             : 
+    1317          94 :   ROS_INFO("[ControlManager]: trackers were loaded");
+    1318             : 
+    1319         659 :   for (int i = 0; i < int(tracker_list_.size()); i++) {
+    1320             : 
+    1321         565 :     std::map<std::string, TrackerParams>::iterator it;
+    1322         565 :     it = trackers_.find(_tracker_names_.at(i));
+    1323             : 
+    1324             :     // create private handlers
+    1325             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1326        1130 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1327             : 
+    1328         565 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1329         565 :     private_handlers->name_space     = it->second.name_space;
+    1330         565 :     private_handlers->runtime_name   = _tracker_names_.at(i);
+    1331         565 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_.at(i));
+    1332             : 
+    1333         565 :     if (_custom_config_ != "") {
+    1334         565 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1335             :     }
+    1336             : 
+    1337         565 :     if (_platform_config_ != "") {
+    1338         565 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1339             :     }
+    1340             : 
+    1341         565 :     if (_world_config_ != "") {
+    1342         565 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1343             :     }
+    1344             : 
+    1345         565 :     if (_network_config_ != "") {
+    1346         565 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1347             :     }
+    1348             : 
+    1349         565 :     bool success = false;
+    1350             : 
+    1351             :     try {
+    1352         565 :       ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+    1353         565 :       success = tracker_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1354             :     }
+    1355           0 :     catch (std::runtime_error& ex) {
+    1356           0 :       ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+    1357             :     }
+    1358             : 
+    1359         565 :     if (!success) {
+    1360           0 :       ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+    1361           0 :       ros::shutdown();
+    1362             :     }
+    1363             :   }
+    1364             : 
+    1365          94 :   ROS_INFO("[ControlManager]: trackers were initialized");
+    1366             : 
+    1367             :   // --------------------------------------------------------------
+    1368             :   // |           check the existance of selected trackers         |
+    1369             :   // --------------------------------------------------------------
+    1370             : 
+    1371             :   // | ------ check for the existance of the hover tracker ------ |
+    1372             : 
+    1373             :   // check if the hover_tracker is within the loaded trackers
+    1374             :   {
+    1375          94 :     auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+    1376             : 
+    1377          94 :     if (idx) {
+    1378          94 :       _ehover_tracker_idx_ = idx.value();
+    1379             :     } else {
+    1380           0 :       ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+    1381           0 :       ros::shutdown();
+    1382             :     }
+    1383             :   }
+    1384             : 
+    1385             :   // | ----- check for the existence of the landoff tracker ----- |
+    1386             : 
+    1387             :   {
+    1388          94 :     auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+    1389             : 
+    1390          94 :     if (idx) {
+    1391          94 :       _landoff_tracker_idx_ = idx.value();
+    1392             :     } else {
+    1393           0 :       ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+    1394           0 :       ros::shutdown();
+    1395             :     }
+    1396             :   }
+    1397             : 
+    1398             :   // | ------- check for the existence of the null tracker ------ |
+    1399             : 
+    1400             :   {
+    1401          94 :     auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+    1402             : 
+    1403          94 :     if (idx) {
+    1404          94 :       _null_tracker_idx_ = idx.value();
+    1405             :     } else {
+    1406           0 :       ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+    1407           0 :       ros::shutdown();
+    1408             :     }
+    1409             :   }
+    1410             : 
+    1411             :   // --------------------------------------------------------------
+    1412             :   // |         check existance of trackers for joystick           |
+    1413             :   // --------------------------------------------------------------
+    1414             : 
+    1415          94 :   if (_joystick_enabled_) {
+    1416             : 
+    1417          94 :     auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+    1418             : 
+    1419          94 :     if (idx) {
+    1420          94 :       _joystick_tracker_idx_ = idx.value();
+    1421             :     } else {
+    1422           0 :       ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+    1423           0 :       ros::shutdown();
+    1424             :     }
+    1425             :   }
+    1426             : 
+    1427          94 :   if (_bumper_switch_tracker_) {
+    1428             : 
+    1429          94 :     auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+    1430             : 
+    1431          94 :     if (!idx) {
+    1432           0 :       ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+    1433           0 :       ros::shutdown();
+    1434             :     }
+    1435             :   }
+    1436             : 
+    1437             :   {
+    1438          94 :     auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+    1439             : 
+    1440          94 :     if (idx) {
+    1441          94 :       _joystick_fallback_tracker_idx_ = idx.value();
+    1442             :     } else {
+    1443           0 :       ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+    1444           0 :       ros::shutdown();
+    1445             :     }
+    1446             :   }
+    1447             : 
+    1448             :   // --------------------------------------------------------------
+    1449             :   // |                    load the controllers                    |
+    1450             :   // --------------------------------------------------------------
+    1451             : 
+    1452         188 :   std::vector<std::string> custom_controllers;
+    1453             : 
+    1454          94 :   param_loader.loadParam("mrs_controllers", _controller_names_);
+    1455          94 :   param_loader.loadParam("controllers", custom_controllers);
+    1456             : 
+    1457          94 :   if (!custom_controllers.empty()) {
+    1458           0 :     _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+    1459             :   }
+    1460             : 
+    1461          94 :   controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+    1462             : 
+    1463             :   // for each controller in the list
+    1464         564 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    1465             : 
+    1466         940 :     std::string controller_name = _controller_names_.at(i);
+    1467             : 
+    1468             :     // load the controller parameters
+    1469         940 :     std::string address;
+    1470         940 :     std::string name_space;
+    1471             :     double      eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+    1472             :     bool        human_switchable;
+    1473         470 :     param_loader.loadParam(controller_name + "/address", address);
+    1474         470 :     param_loader.loadParam(controller_name + "/namespace", name_space);
+    1475         470 :     param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+    1476         470 :     param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+    1477         470 :     param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+    1478         470 :     param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+    1479             : 
+    1480             :     // check if the controller can output some of the required outputs
+    1481             :     {
+    1482             : 
+    1483         470 :       ControlOutputModalities_t outputs;
+    1484         470 :       param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+    1485         470 :       param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+    1486         470 :       param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+    1487         470 :       param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+    1488         470 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+    1489         470 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+    1490         470 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+    1491         470 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+    1492         470 :       param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+    1493             : 
+    1494         470 :       bool meets_actuators             = (_hw_api_inputs_.actuators && outputs.actuators);
+    1495         470 :       bool meets_control_group         = (_hw_api_inputs_.control_group && outputs.control_group);
+    1496         470 :       bool meets_attitude_rate         = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+    1497         470 :       bool meets_attitude              = (_hw_api_inputs_.attitude && outputs.attitude);
+    1498         470 :       bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+    1499         470 :       bool meets_acceleration_hdg      = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+    1500         470 :       bool meets_velocity_hdg_rate     = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+    1501         470 :       bool meets_velocity_hdg          = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+    1502         470 :       bool meets_position              = (_hw_api_inputs_.position && outputs.position);
+    1503             : 
+    1504         455 :       bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+    1505         925 :                                 meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+    1506             : 
+    1507         470 :       if (!meets_requirements) {
+    1508             : 
+    1509           0 :         ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+    1510             :                   controller_name.c_str());
+    1511             : 
+    1512           0 :         if (_hw_api_inputs_.actuators) {
+    1513           0 :           ROS_ERROR("[ControlManager]: - actuators");
+    1514             :         }
+    1515             : 
+    1516           0 :         if (_hw_api_inputs_.control_group) {
+    1517           0 :           ROS_ERROR("[ControlManager]: - control group");
+    1518             :         }
+    1519             : 
+    1520           0 :         if (_hw_api_inputs_.attitude_rate) {
+    1521           0 :           ROS_ERROR("[ControlManager]: - attitude rate");
+    1522             :         }
+    1523             : 
+    1524           0 :         if (_hw_api_inputs_.attitude) {
+    1525           0 :           ROS_ERROR("[ControlManager]: - attitude");
+    1526             :         }
+    1527             : 
+    1528           0 :         if (_hw_api_inputs_.acceleration_hdg_rate) {
+    1529           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+    1530             :         }
+    1531             : 
+    1532           0 :         if (_hw_api_inputs_.acceleration_hdg) {
+    1533           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg");
+    1534             :         }
+    1535             : 
+    1536           0 :         if (_hw_api_inputs_.velocity_hdg_rate) {
+    1537           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+    1538             :         }
+    1539             : 
+    1540           0 :         if (_hw_api_inputs_.velocity_hdg) {
+    1541           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg");
+    1542             :         }
+    1543             : 
+    1544           0 :         if (_hw_api_inputs_.position) {
+    1545           0 :           ROS_ERROR("[ControlManager]: - position");
+    1546             :         }
+    1547             : 
+    1548           0 :         ros::shutdown();
+    1549             :       }
+    1550             : 
+    1551         470 :       if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+    1552           0 :         ROS_ERROR(
+    1553             :             "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+    1554           0 :         ros::shutdown();
+    1555             :       }
+    1556             :     }
+    1557             : 
+    1558             :     // | --- alter the timer rates based on the hw capabilities --- |
+    1559             : 
+    1560         470 :     CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+    1561             : 
+    1562         470 :     if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+    1563          30 :       _safety_timer_rate_     = 200.0;
+    1564          30 :       desired_uav_state_rate_ = 250.0;
+    1565         440 :     } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+    1566         350 :       _safety_timer_rate_     = 100.0;
+    1567         350 :       desired_uav_state_rate_ = 100.0;
+    1568          90 :     } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+    1569          20 :       _safety_timer_rate_     = 30.0;
+    1570          20 :       _status_timer_rate_     = 1.0;
+    1571          20 :       desired_uav_state_rate_ = 40.0;
+    1572             : 
+    1573          20 :       if (_uav_state_max_missing_time_ < 0.2) {
+    1574           4 :         _uav_state_max_missing_time_ = 0.2;
+    1575             :       }
+    1576          70 :     } else if (lowest_output >= VELOCITY_HDG_RATE) {
+    1577          70 :       _safety_timer_rate_     = 20.0;
+    1578          70 :       _status_timer_rate_     = 1.0;
+    1579          70 :       desired_uav_state_rate_ = 20.0;
+    1580             : 
+    1581          70 :       if (_uav_state_max_missing_time_ < 1.0) {
+    1582          14 :         _uav_state_max_missing_time_ = 1.0;
+    1583             :       }
+    1584             :     }
+    1585             : 
+    1586         470 :     if (eland_threshold == 0) {
+    1587          95 :       eland_threshold = 1e6;
+    1588             :     }
+    1589             : 
+    1590         470 :     if (failsafe_threshold == 0) {
+    1591          95 :       failsafe_threshold = 1e6;
+    1592             :     }
+    1593             : 
+    1594         470 :     if (odometry_innovation_threshold == 0) {
+    1595          96 :       odometry_innovation_threshold = 1e6;
+    1596             :     }
+    1597             : 
+    1598        1410 :     ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+    1599         470 :     controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+    1600             : 
+    1601             :     try {
+    1602         470 :       ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+    1603         470 :       controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+    1604             :     }
+    1605           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1606           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+    1607           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1608           0 :       ros::shutdown();
+    1609             :     }
+    1610           0 :     catch (pluginlib::PluginlibException& ex) {
+    1611           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+    1612           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1613           0 :       ros::shutdown();
+    1614             :     }
+    1615             :   }
+    1616             : 
+    1617          94 :   ROS_INFO("[ControlManager]: controllers were loaded");
+    1618             : 
+    1619         564 :   for (int i = 0; i < int(controller_list_.size()); i++) {
+    1620             : 
+    1621         470 :     std::map<std::string, ControllerParams>::iterator it;
+    1622         470 :     it = controllers_.find(_controller_names_.at(i));
+    1623             : 
+    1624             :     // create private handlers
+    1625             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1626         940 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1627             : 
+    1628         470 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1629         470 :     private_handlers->name_space     = it->second.name_space;
+    1630         470 :     private_handlers->runtime_name   = _controller_names_.at(i);
+    1631         470 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_.at(i));
+    1632             : 
+    1633         470 :     if (_custom_config_ != "") {
+    1634         470 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1635             :     }
+    1636             : 
+    1637         470 :     if (_platform_config_ != "") {
+    1638         470 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1639             :     }
+    1640             : 
+    1641         470 :     if (_world_config_ != "") {
+    1642         470 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1643             :     }
+    1644             : 
+    1645         470 :     if (_network_config_ != "") {
+    1646         470 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1647             :     }
+    1648             : 
+    1649         470 :     bool success = false;
+    1650             : 
+    1651             :     try {
+    1652             : 
+    1653         470 :       ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+    1654         470 :       success = controller_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1655             :     }
+    1656           0 :     catch (std::runtime_error& ex) {
+    1657           0 :       ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+    1658             :     }
+    1659             : 
+    1660         470 :     if (!success) {
+    1661           0 :       ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+    1662           0 :       ros::shutdown();
+    1663             :     }
+    1664             :   }
+    1665             : 
+    1666          94 :   ROS_INFO("[ControlManager]: controllers were initialized");
+    1667             : 
+    1668             :   {
+    1669          94 :     auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+    1670             : 
+    1671          94 :     if (idx) {
+    1672          94 :       _failsafe_controller_idx_ = idx.value();
+    1673             :     } else {
+    1674           0 :       ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+    1675           0 :       ros::shutdown();
+    1676             :     }
+    1677             :   }
+    1678             : 
+    1679             :   {
+    1680          94 :     auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+    1681             : 
+    1682          94 :     if (idx) {
+    1683          94 :       _eland_controller_idx_ = idx.value();
+    1684             :     } else {
+    1685           0 :       ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+    1686           0 :       ros::shutdown();
+    1687             :     }
+    1688             :   }
+    1689             : 
+    1690             :   {
+    1691          94 :     auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+    1692             : 
+    1693          94 :     if (idx) {
+    1694          94 :       _joystick_controller_idx_ = idx.value();
+    1695             :     } else {
+    1696           0 :       ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+    1697           0 :       ros::shutdown();
+    1698             :     }
+    1699             :   }
+    1700             : 
+    1701          94 :   if (_bumper_switch_controller_) {
+    1702             : 
+    1703          94 :     auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+    1704             : 
+    1705          94 :     if (!idx) {
+    1706           0 :       ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+    1707           0 :       ros::shutdown();
+    1708             :     }
+    1709             :   }
+    1710             : 
+    1711             :   {
+    1712          94 :     auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+    1713             : 
+    1714          94 :     if (idx) {
+    1715          94 :       _joystick_fallback_controller_idx_ = idx.value();
+    1716             :     } else {
+    1717           0 :       ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+    1718           0 :       ros::shutdown();
+    1719             :     }
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                  activate the NullTracker                  |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726          94 :   ROS_INFO("[ControlManager]: activating the null tracker");
+    1727             : 
+    1728          94 :   tracker_list_.at(_null_tracker_idx_)->activate(last_tracker_cmd_);
+    1729          94 :   active_tracker_idx_ = _null_tracker_idx_;
+    1730             : 
+    1731             :   // --------------------------------------------------------------
+    1732             :   // |    activate the eland controller as the first controller   |
+    1733             :   // --------------------------------------------------------------
+    1734             : 
+    1735          94 :   ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_.at(_eland_controller_idx_).c_str());
+    1736             : 
+    1737          94 :   controller_list_.at(_eland_controller_idx_)->activate(last_control_output_);
+    1738          94 :   active_controller_idx_ = _eland_controller_idx_;
+    1739             : 
+    1740             :   // update the time
+    1741             :   {
+    1742         188 :     std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    1743             : 
+    1744          94 :     controller_tracker_switch_time_ = ros::Time::now();
+    1745             :   }
+    1746             : 
+    1747          94 :   output_enabled_ = false;
+    1748             : 
+    1749             :   // | --------------- set the default constraints -------------- |
+    1750             : 
+    1751          94 :   sanitized_constraints_ = current_constraints_;
+    1752          94 :   setConstraints(current_constraints_);
+    1753             : 
+    1754             :   // | ------------------------ profiler ------------------------ |
+    1755             : 
+    1756          94 :   profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+    1757             : 
+    1758             :   // | ----------------------- publishers ----------------------- |
+    1759             : 
+    1760          94 :   control_output_publisher_ = OutputPublisher(nh_);
+    1761             : 
+    1762          94 :   ph_controller_diagnostics_             = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+    1763          94 :   ph_tracker_cmd_                        = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+    1764          94 :   ph_mrs_odom_input_                     = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+    1765          94 :   ph_control_reference_odom_             = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+    1766          94 :   ph_diagnostics_                        = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+    1767          94 :   ph_offboard_on_                        = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+    1768          94 :   ph_tilt_error_                         = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+    1769          94 :   ph_mass_estimate_                      = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+    1770          94 :   ph_mass_nominal_                       = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_nominal_out", 1, true);
+    1771          94 :   ph_throttle_                           = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+    1772          94 :   ph_thrust_                             = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+    1773          94 :   ph_control_error_                      = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+    1774          94 :   ph_safety_area_markers_                = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+    1775          94 :   ph_safety_area_coordinates_markers_    = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+    1776          94 :   ph_disturbances_markers_               = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+    1777          94 :   ph_current_constraints_                = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+    1778          94 :   ph_heading_                            = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+    1779          94 :   ph_speed_                              = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+    1780          94 :   pub_debug_original_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+    1781          94 :   pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+    1782             : 
+    1783             :   // | ------------------ publish nominal mass ------------------ |
+    1784             : 
+    1785             :   {
+    1786          94 :     std_msgs::Float64 nominal_mass;
+    1787             : 
+    1788          94 :     nominal_mass.data = _uav_mass_;
+    1789             : 
+    1790          94 :     ph_mass_nominal_.publish(nominal_mass);
+    1791             :   }
+    1792             : 
+    1793             :   // | ----------------------- subscribers ---------------------- |
+    1794             : 
+    1795         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+    1796          94 :   shopts.nh                 = nh_;
+    1797          94 :   shopts.node_name          = "ControlManager";
+    1798          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+    1799          94 :   shopts.threadsafe         = true;
+    1800          94 :   shopts.autostart          = true;
+    1801          94 :   shopts.queue_size         = 10;
+    1802          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+    1803             : 
+    1804          94 :   if (_state_input_ == INPUT_UAV_STATE) {
+    1805          94 :     sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+    1806           0 :   } else if (_state_input_ == INPUT_ODOMETRY) {
+    1807           0 :     sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+    1808             :   }
+    1809             : 
+    1810          94 :   if (_odometry_innovation_check_enabled_) {
+    1811          94 :     sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+    1812             :   }
+    1813             : 
+    1814          94 :   sh_bumper_    = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+    1815          94 :   sh_max_z_     = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+    1816          94 :   sh_joystick_  = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+    1817          94 :   sh_gnss_      = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+    1818          94 :   sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+    1819             : 
+    1820          94 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+    1821             : 
+    1822             :   // | -------------------- general services -------------------- |
+    1823             : 
+    1824          94 :   service_server_switch_tracker_             = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+    1825          94 :   service_server_switch_controller_          = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+    1826          94 :   service_server_reset_tracker_              = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+    1827          94 :   service_server_hover_                      = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+    1828          94 :   service_server_ehover_                     = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+    1829          94 :   service_server_failsafe_                   = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+    1830          94 :   service_server_failsafe_escalating_        = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+    1831          94 :   service_server_toggle_output_              = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+    1832          94 :   service_server_arm_                        = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+    1833          94 :   service_server_enable_callbacks_           = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+    1834          94 :   service_server_set_constraints_            = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+    1835          94 :   service_server_use_joystick_               = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+    1836          94 :   service_server_use_safety_area_            = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+    1837          94 :   service_server_eland_                      = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+    1838          94 :   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+    1839          94 :   service_server_set_min_z_                  = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this);
+    1840          94 :   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+    1841          94 :   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+    1842          94 :   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+    1843          94 :   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+    1844          94 :   service_server_get_min_z_                  = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+    1845          94 :   service_server_validate_reference_         = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+    1846          94 :   service_server_validate_reference_2d_      = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+    1847          94 :   service_server_validate_reference_list_    = nh_.advertiseService("validate_reference_list_in", &ControlManager::callbackValidateReferenceList, this);
+    1848          94 :   service_server_start_trajectory_tracking_  = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+    1849          94 :   service_server_stop_trajectory_tracking_   = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+    1850          94 :   service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+    1851          94 :   service_server_goto_trajectory_start_      = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+    1852             : 
+    1853          94 :   sch_arming_                 = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+    1854          94 :   sch_eland_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+    1855          94 :   sch_shutdown_               = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+    1856          94 :   sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+    1857          94 :   sch_ungrip_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+    1858          94 :   sch_parachute_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+    1859             : 
+    1860             :   // | ---------------- setpoint command services --------------- |
+    1861             : 
+    1862             :   // human callable
+    1863          94 :   service_server_goto_                 = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+    1864          94 :   service_server_goto_fcu_             = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+    1865          94 :   service_server_goto_relative_        = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+    1866          94 :   service_server_goto_altitude_        = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+    1867          94 :   service_server_set_heading_          = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+    1868          94 :   service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+    1869             : 
+    1870          94 :   service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+    1871          94 :   sh_reference_             = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+    1872             : 
+    1873          94 :   service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+    1874             :   sh_velocity_reference_ =
+    1875          94 :       mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+    1876             : 
+    1877          94 :   service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+    1878             :   sh_trajectory_reference_ =
+    1879          94 :       mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+    1880             : 
+    1881             :   // | --------------------- other services --------------------- |
+    1882             : 
+    1883          94 :   service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+    1884          94 :   service_server_pirouette_           = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+    1885             : 
+    1886             :   // | ------------------------- timers ------------------------- |
+    1887             : 
+    1888          94 :   timer_status_    = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+    1889          94 :   timer_safety_    = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+    1890          94 :   timer_bumper_    = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerBumper, this);
+    1891          94 :   timer_eland_     = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+    1892          94 :   timer_failsafe_  = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+    1893          94 :   timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+    1894          94 :   timer_joystick_  = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+    1895             : 
+    1896             :   // | ----------------------- finish init ---------------------- |
+    1897             : 
+    1898          94 :   if (!param_loader.loadedSuccessfully()) {
+    1899           0 :     ROS_ERROR("[ControlManager]: could not load all parameters!");
+    1900           0 :     ros::shutdown();
+    1901             :   }
+    1902             : 
+    1903          94 :   is_initialized_ = true;
+    1904             : 
+    1905          94 :   ROS_INFO("[ControlManager]: initialized");
+    1906          94 : }
+    1907             : 
+    1908             : //}
+    1909             : 
+    1910             : // --------------------------------------------------------------
+    1911             : // |                           timers                           |
+    1912             : // --------------------------------------------------------------
+    1913             : 
+    1914             : /* timerHwApiCapabilities() //{ */
+    1915             : 
+    1916         157 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+    1917             : 
+    1918         314 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+    1919         314 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+    1920             : 
+    1921         157 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+    1922          63 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+    1923          63 :     return;
+    1924             :   }
+    1925             : 
+    1926         188 :   auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+    1927             : 
+    1928          94 :   ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+    1929             : 
+    1930          94 :   if (hw_ap_capabilities->accepts_actuator_cmd) {
+    1931           3 :     ROS_INFO("[ControlManager]: - actuator command");
+    1932           3 :     _hw_api_inputs_.actuators = true;
+    1933             :   }
+    1934             : 
+    1935          94 :   if (hw_ap_capabilities->accepts_control_group_cmd) {
+    1936           3 :     ROS_INFO("[ControlManager]: - control group command");
+    1937           3 :     _hw_api_inputs_.control_group = true;
+    1938             :   }
+    1939             : 
+    1940          94 :   if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+    1941          67 :     ROS_INFO("[ControlManager]: - attitude rate command");
+    1942          67 :     _hw_api_inputs_.attitude_rate = true;
+    1943             :   }
+    1944             : 
+    1945          94 :   if (hw_ap_capabilities->accepts_attitude_cmd) {
+    1946          65 :     ROS_INFO("[ControlManager]: - attitude command");
+    1947          65 :     _hw_api_inputs_.attitude = true;
+    1948             :   }
+    1949             : 
+    1950          94 :   if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+    1951           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+    1952           2 :     _hw_api_inputs_.acceleration_hdg_rate = true;
+    1953             :   }
+    1954             : 
+    1955          94 :   if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+    1956           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg command");
+    1957           2 :     _hw_api_inputs_.acceleration_hdg = true;
+    1958             :   }
+    1959             : 
+    1960          94 :   if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+    1961           8 :     ROS_INFO("[ControlManager]: - velocityhdg rate command");
+    1962           8 :     _hw_api_inputs_.velocity_hdg_rate = true;
+    1963             :   }
+    1964             : 
+    1965          94 :   if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+    1966           4 :     ROS_INFO("[ControlManager]: - velocityhdg command");
+    1967           4 :     _hw_api_inputs_.velocity_hdg = true;
+    1968             :   }
+    1969             : 
+    1970          94 :   if (hw_ap_capabilities->accepts_position_cmd) {
+    1971           2 :     ROS_INFO("[ControlManager]: - position command");
+    1972           2 :     _hw_api_inputs_.position = true;
+    1973             :   }
+    1974             : 
+    1975          94 :   initialize();
+    1976             : 
+    1977          94 :   timer_hw_api_capabilities_.stop();
+    1978             : }
+    1979             : 
+    1980             : //}
+    1981             : 
+    1982             : /* //{ timerStatus() */
+    1983             : 
+    1984       17162 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+    1985             : 
+    1986       17162 :   if (!is_initialized_)
+    1987           0 :     return;
+    1988             : 
+    1989       51486 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+    1990       51486 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+    1991             : 
+    1992             :   // copy member variables
+    1993       34324 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1994       34324 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1995       17162 :   auto yaw_error             = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+    1996       17162 :   auto position_error        = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    1997       17162 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    1998       17162 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    1999             : 
+    2000             :   double uav_x, uav_y, uav_z;
+    2001       17162 :   uav_x = uav_state.pose.position.x;
+    2002       17162 :   uav_y = uav_state.pose.position.y;
+    2003       17162 :   uav_z = uav_state.pose.position.z;
+    2004             : 
+    2005             :   // --------------------------------------------------------------
+    2006             :   // |                      print the status                      |
+    2007             :   // --------------------------------------------------------------
+    2008             : 
+    2009             :   {
+    2010       34324 :     std::string controller = _controller_names_.at(active_controller_idx);
+    2011       34324 :     std::string tracker    = _tracker_names_.at(active_tracker_idx);
+    2012       17162 :     double      mass       = last_control_output.diagnostics.total_mass;
+    2013       17162 :     double      bx_b       = last_control_output.diagnostics.disturbance_bx_b;
+    2014       17162 :     double      by_b       = last_control_output.diagnostics.disturbance_by_b;
+    2015       17162 :     double      wx_w       = last_control_output.diagnostics.disturbance_wx_w;
+    2016       17162 :     double      wy_w       = last_control_output.diagnostics.disturbance_wy_w;
+    2017             : 
+    2018       17162 :     ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+    2019             :                       tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+    2020             :   }
+    2021             : 
+    2022             :   // --------------------------------------------------------------
+    2023             :   // |                   publish the diagnostics                  |
+    2024             :   // --------------------------------------------------------------
+    2025             : 
+    2026       17162 :   publishDiagnostics();
+    2027             : 
+    2028             :   // --------------------------------------------------------------
+    2029             :   // |                publish if the offboard is on               |
+    2030             :   // --------------------------------------------------------------
+    2031             : 
+    2032       17162 :   if (offboard_mode_) {
+    2033             : 
+    2034       11776 :     std_msgs::Empty offboard_on_out;
+    2035             : 
+    2036       11776 :     ph_offboard_on_.publish(offboard_on_out);
+    2037             :   }
+    2038             : 
+    2039             :   // --------------------------------------------------------------
+    2040             :   // |                   publish the tilt error                   |
+    2041             :   // --------------------------------------------------------------
+    2042             :   {
+    2043       34324 :     std::scoped_lock lock(mutex_attitude_error_);
+    2044             : 
+    2045       17162 :     if (tilt_error_) {
+    2046             : 
+    2047       34324 :       mrs_msgs::Float64Stamped tilt_error_out;
+    2048       17162 :       tilt_error_out.header.stamp    = ros::Time::now();
+    2049       17162 :       tilt_error_out.header.frame_id = uav_state.header.frame_id;
+    2050       17162 :       tilt_error_out.value           = (180.0 / M_PI) * tilt_error_.value();
+    2051             : 
+    2052       17162 :       ph_tilt_error_.publish(tilt_error_out);
+    2053             :     }
+    2054             :   }
+    2055             : 
+    2056             :   // --------------------------------------------------------------
+    2057             :   // |                  publish the control error                 |
+    2058             :   // --------------------------------------------------------------
+    2059             : 
+    2060       17162 :   if (position_error) {
+    2061             : 
+    2062       10674 :     Eigen::Vector3d pos_error_value = position_error.value();
+    2063             : 
+    2064       21348 :     mrs_msgs::ControlError msg_out;
+    2065             : 
+    2066       10674 :     msg_out.header.stamp    = ros::Time::now();
+    2067       10674 :     msg_out.header.frame_id = uav_state.header.frame_id;
+    2068             : 
+    2069       10674 :     msg_out.position_errors.x    = pos_error_value(0);
+    2070       10674 :     msg_out.position_errors.y    = pos_error_value(1);
+    2071       10674 :     msg_out.position_errors.z    = pos_error_value(2);
+    2072       10674 :     msg_out.total_position_error = pos_error_value.norm();
+    2073             : 
+    2074       10674 :     if (yaw_error_) {
+    2075       10674 :       msg_out.yaw_error = yaw_error.value();
+    2076             :     }
+    2077             : 
+    2078       10674 :     std::map<std::string, ControllerParams>::iterator it;
+    2079             : 
+    2080       10674 :     it = controllers_.find(_controller_names_.at(active_controller_idx));
+    2081             : 
+    2082       10674 :     msg_out.position_eland_threshold    = it->second.eland_threshold;
+    2083       10674 :     msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+    2084             : 
+    2085       10674 :     ph_control_error_.publish(msg_out);
+    2086             :   }
+    2087             : 
+    2088             :   // --------------------------------------------------------------
+    2089             :   // |                  publish the mass estimate                 |
+    2090             :   // --------------------------------------------------------------
+    2091             : 
+    2092       17162 :   if (last_control_output.diagnostics.mass_estimator) {
+    2093             : 
+    2094        9363 :     std_msgs::Float64 mass_estimate_out;
+    2095        9363 :     mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    2096             : 
+    2097        9363 :     ph_mass_estimate_.publish(mass_estimate_out);
+    2098             :   }
+    2099             : 
+    2100             :   // --------------------------------------------------------------
+    2101             :   // |                 publish the current heading                |
+    2102             :   // --------------------------------------------------------------
+    2103             : 
+    2104       17162 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2105             : 
+    2106             :     try {
+    2107             : 
+    2108             :       double heading;
+    2109             : 
+    2110       14612 :       heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2111             : 
+    2112       29224 :       mrs_msgs::Float64Stamped heading_out;
+    2113       14612 :       heading_out.header = uav_state.header;
+    2114       14612 :       heading_out.value  = heading;
+    2115             : 
+    2116       14612 :       ph_heading_.publish(heading_out);
+    2117             :     }
+    2118           0 :     catch (...) {
+    2119           0 :       ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+    2120             :     }
+    2121             :   }
+    2122             : 
+    2123             :   // --------------------------------------------------------------
+    2124             :   // |                  publish the current speed                 |
+    2125             :   // --------------------------------------------------------------
+    2126             : 
+    2127       17162 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2128             : 
+    2129       14612 :     double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+    2130             : 
+    2131       29224 :     mrs_msgs::Float64Stamped speed_out;
+    2132       14612 :     speed_out.header = uav_state.header;
+    2133       14612 :     speed_out.value  = speed;
+    2134             : 
+    2135       14612 :     ph_speed_.publish(speed_out);
+    2136             :   }
+    2137             : 
+    2138             :   // --------------------------------------------------------------
+    2139             :   // |               publish the safety area markers              |
+    2140             :   // --------------------------------------------------------------
+    2141             : 
+    2142       17162 :   if (use_safety_area_) {
+    2143             : 
+    2144       27004 :     mrs_msgs::ReferenceStamped temp_ref;
+    2145       13502 :     temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+    2146             : 
+    2147       27004 :     geometry_msgs::TransformStamped tf;
+    2148             : 
+    2149       40506 :     auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+    2150             : 
+    2151       13502 :     if (ret) {
+    2152             : 
+    2153       11258 :       ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+    2154             : 
+    2155       22516 :       visualization_msgs::MarkerArray safety_area_marker_array;
+    2156       22516 :       visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+    2157             : 
+    2158       22516 :       mrs_lib::Polygon border = safety_zone_->getBorder();
+    2159             : 
+    2160       22516 :       std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+    2161       22516 :       std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+    2162             : 
+    2163       22516 :       std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+    2164       22516 :       std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+    2165             : 
+    2166             :       // if we fail in transforming the area at some point
+    2167             :       // do not publish it at all
+    2168       11258 :       bool tf_success = true;
+    2169             : 
+    2170       22516 :       geometry_msgs::TransformStamped tf = ret.value();
+    2171             : 
+    2172             :       /* transform area points to local origin //{ */
+    2173             : 
+    2174             :       // transform border bottom points to local origin
+    2175       56290 :       for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+    2176             : 
+    2177       45032 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2178       45032 :         temp_ref.header.stamp         = ros::Time(0);
+    2179       45032 :         temp_ref.reference.position.x = border_points_bot_original.at(i).x;
+    2180       45032 :         temp_ref.reference.position.y = border_points_bot_original.at(i).y;
+    2181       45032 :         temp_ref.reference.position.z = border_points_bot_original.at(i).z;
+    2182             : 
+    2183       90064 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2184             : 
+    2185       45032 :           temp_ref = ret.value();
+    2186             : 
+    2187       45032 :           border_points_bot_transformed.at(i).x = temp_ref.reference.position.x;
+    2188       45032 :           border_points_bot_transformed.at(i).y = temp_ref.reference.position.y;
+    2189       45032 :           border_points_bot_transformed.at(i).z = temp_ref.reference.position.z;
+    2190             : 
+    2191             :         } else {
+    2192           0 :           tf_success = false;
+    2193             :         }
+    2194             :       }
+    2195             : 
+    2196             :       // transform border top points to local origin
+    2197       56290 :       for (size_t i = 0; i < border_points_top_original.size(); i++) {
+    2198             : 
+    2199       45032 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2200       45032 :         temp_ref.header.stamp         = ros::Time(0);
+    2201       45032 :         temp_ref.reference.position.x = border_points_top_original.at(i).x;
+    2202       45032 :         temp_ref.reference.position.y = border_points_top_original.at(i).y;
+    2203       45032 :         temp_ref.reference.position.z = border_points_top_original.at(i).z;
+    2204             : 
+    2205       90064 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2206             : 
+    2207       45032 :           temp_ref = ret.value();
+    2208             : 
+    2209       45032 :           border_points_top_transformed.at(i).x = temp_ref.reference.position.x;
+    2210       45032 :           border_points_top_transformed.at(i).y = temp_ref.reference.position.y;
+    2211       45032 :           border_points_top_transformed.at(i).z = temp_ref.reference.position.z;
+    2212             : 
+    2213             :         } else {
+    2214           0 :           tf_success = false;
+    2215             :         }
+    2216             :       }
+    2217             : 
+    2218             :       //}
+    2219             : 
+    2220       22516 :       visualization_msgs::Marker safety_area_marker;
+    2221             : 
+    2222       11258 :       safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2223       11258 :       safety_area_marker.type            = visualization_msgs::Marker::LINE_LIST;
+    2224       11258 :       safety_area_marker.color.a         = 0.15;
+    2225       11258 :       safety_area_marker.scale.x         = 0.2;
+    2226       11258 :       safety_area_marker.color.r         = 1;
+    2227       11258 :       safety_area_marker.color.g         = 0;
+    2228       11258 :       safety_area_marker.color.b         = 0;
+    2229             : 
+    2230       11258 :       safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2231             : 
+    2232       22516 :       visualization_msgs::Marker safety_area_coordinates_marker;
+    2233             : 
+    2234       11258 :       safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2235       11258 :       safety_area_coordinates_marker.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    2236       11258 :       safety_area_coordinates_marker.color.a         = 1;
+    2237       11258 :       safety_area_coordinates_marker.scale.z         = 1.0;
+    2238       11258 :       safety_area_coordinates_marker.color.r         = 0;
+    2239       11258 :       safety_area_coordinates_marker.color.g         = 0;
+    2240       11258 :       safety_area_coordinates_marker.color.b         = 0;
+    2241             : 
+    2242       11258 :       safety_area_coordinates_marker.id = 0;
+    2243             : 
+    2244       11258 :       safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2245             : 
+    2246             :       /* adding safety area points //{ */
+    2247             : 
+    2248             :       // bottom border
+    2249       56290 :       for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+    2250             : 
+    2251       45032 :         safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+    2252       45032 :         safety_area_marker.points.push_back(border_points_bot_transformed.at((i + 1) % border_points_bot_transformed.size()));
+    2253             : 
+    2254       90064 :         std::stringstream ss;
+    2255             : 
+    2256       45032 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2257           0 :           ss << "idx: " << i << std::endl
+    2258           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+    2259           0 :              << "lon: " << border_points_bot_original.at(i).y;
+    2260             :         } else {
+    2261       45032 :           ss << "idx: " << i << std::endl
+    2262       45032 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+    2263       45032 :              << "y: " << border_points_bot_original.at(i).y;
+    2264             :         }
+    2265             : 
+    2266       45032 :         safety_area_coordinates_marker.color.r = 0;
+    2267       45032 :         safety_area_coordinates_marker.color.g = 0;
+    2268       45032 :         safety_area_coordinates_marker.color.b = 0;
+    2269             : 
+    2270       45032 :         safety_area_coordinates_marker.pose.position = border_points_bot_transformed.at(i);
+    2271       45032 :         safety_area_coordinates_marker.text          = ss.str();
+    2272       45032 :         safety_area_coordinates_marker.id++;
+    2273             : 
+    2274       45032 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2275             :       }
+    2276             : 
+    2277             :       // top border + top/bot edges
+    2278       56290 :       for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+    2279             : 
+    2280       45032 :         safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+    2281       45032 :         safety_area_marker.points.push_back(border_points_top_transformed.at((i + 1) % border_points_top_transformed.size()));
+    2282             : 
+    2283       45032 :         safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+    2284       45032 :         safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+    2285             : 
+    2286       90064 :         std::stringstream ss;
+    2287             : 
+    2288       45032 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2289           0 :           ss << "idx: " << i << std::endl
+    2290           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+    2291           0 :              << "lon: " << border_points_bot_original.at(i).y;
+    2292             :         } else {
+    2293       45032 :           ss << "idx: " << i << std::endl
+    2294       45032 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+    2295       45032 :              << "y: " << border_points_bot_original.at(i).y;
+    2296             :         }
+    2297             : 
+    2298       45032 :         safety_area_coordinates_marker.color.r = 1;
+    2299       45032 :         safety_area_coordinates_marker.color.g = 1;
+    2300       45032 :         safety_area_coordinates_marker.color.b = 1;
+    2301             : 
+    2302       45032 :         safety_area_coordinates_marker.pose.position = border_points_top_transformed.at(i);
+    2303       45032 :         safety_area_coordinates_marker.text          = ss.str();
+    2304       45032 :         safety_area_coordinates_marker.id++;
+    2305             : 
+    2306       45032 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2307             :       }
+    2308             : 
+    2309             :       //}
+    2310             : 
+    2311       11258 :       if (tf_success) {
+    2312             : 
+    2313       11258 :         safety_area_marker_array.markers.push_back(safety_area_marker);
+    2314             : 
+    2315       11258 :         ph_safety_area_markers_.publish(safety_area_marker_array);
+    2316             : 
+    2317       11258 :         ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+    2318             :       }
+    2319             : 
+    2320             :     } else {
+    2321        2244 :       ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+    2322             :     }
+    2323             :   }
+    2324             : 
+    2325             :   // --------------------------------------------------------------
+    2326             :   // |              publish the disturbances markers              |
+    2327             :   // --------------------------------------------------------------
+    2328             : 
+    2329       17162 :   if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+    2330             : 
+    2331       18826 :     visualization_msgs::MarkerArray msg_out;
+    2332             : 
+    2333        9413 :     double id = 0;
+    2334             : 
+    2335        9413 :     double multiplier = 1.0;
+    2336             : 
+    2337        9413 :     Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2338             : 
+    2339        9413 :     Eigen::Vector3d      vec3d;
+    2340        9413 :     geometry_msgs::Point point;
+    2341             : 
+    2342             :     /* world disturbance //{ */
+    2343             :     {
+    2344             : 
+    2345       18826 :       visualization_msgs::Marker marker;
+    2346             : 
+    2347        9413 :       marker.header.frame_id = uav_state.header.frame_id;
+    2348        9413 :       marker.header.stamp    = ros::Time::now();
+    2349        9413 :       marker.ns              = "control_manager";
+    2350        9413 :       marker.id              = id++;
+    2351        9413 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2352        9413 :       marker.action          = visualization_msgs::Marker::ADD;
+    2353             : 
+    2354             :       /* position //{ */
+    2355             : 
+    2356        9413 :       marker.pose.position.x = 0.0;
+    2357        9413 :       marker.pose.position.y = 0.0;
+    2358        9413 :       marker.pose.position.z = 0.0;
+    2359             : 
+    2360             :       //}
+    2361             : 
+    2362             :       /* orientation //{ */
+    2363             : 
+    2364        9413 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2365             : 
+    2366             :       //}
+    2367             : 
+    2368             :       /* origin //{ */
+    2369        9413 :       point.x = uav_x;
+    2370        9413 :       point.y = uav_y;
+    2371        9413 :       point.z = uav_z;
+    2372             : 
+    2373        9413 :       marker.points.push_back(point);
+    2374             : 
+    2375             :       //}
+    2376             : 
+    2377             :       /* tip //{ */
+    2378             : 
+    2379        9413 :       point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+    2380        9413 :       point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+    2381        9413 :       point.z = uav_z;
+    2382             : 
+    2383        9413 :       marker.points.push_back(point);
+    2384             : 
+    2385             :       //}
+    2386             : 
+    2387        9413 :       marker.scale.x = 0.05;
+    2388        9413 :       marker.scale.y = 0.05;
+    2389        9413 :       marker.scale.z = 0.05;
+    2390             : 
+    2391        9413 :       marker.color.a = 0.5;
+    2392        9413 :       marker.color.r = 1.0;
+    2393        9413 :       marker.color.g = 0.0;
+    2394        9413 :       marker.color.b = 0.0;
+    2395             : 
+    2396        9413 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2397             : 
+    2398        9413 :       msg_out.markers.push_back(marker);
+    2399             :     }
+    2400             : 
+    2401             :     //}
+    2402             : 
+    2403             :     /* body disturbance //{ */
+    2404             :     {
+    2405             : 
+    2406       18826 :       visualization_msgs::Marker marker;
+    2407             : 
+    2408        9413 :       marker.header.frame_id = uav_state.header.frame_id;
+    2409        9413 :       marker.header.stamp    = ros::Time::now();
+    2410        9413 :       marker.ns              = "control_manager";
+    2411        9413 :       marker.id              = id++;
+    2412        9413 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2413        9413 :       marker.action          = visualization_msgs::Marker::ADD;
+    2414             : 
+    2415             :       /* position //{ */
+    2416             : 
+    2417        9413 :       marker.pose.position.x = 0.0;
+    2418        9413 :       marker.pose.position.y = 0.0;
+    2419        9413 :       marker.pose.position.z = 0.0;
+    2420             : 
+    2421             :       //}
+    2422             : 
+    2423             :       /* orientation //{ */
+    2424             : 
+    2425        9413 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2426             : 
+    2427             :       //}
+    2428             : 
+    2429             :       /* origin //{ */
+    2430             : 
+    2431        9413 :       point.x = uav_x;
+    2432        9413 :       point.y = uav_y;
+    2433        9413 :       point.z = uav_z;
+    2434             : 
+    2435        9413 :       marker.points.push_back(point);
+    2436             : 
+    2437             :       //}
+    2438             : 
+    2439             :       /* tip //{ */
+    2440             : 
+    2441        9413 :       vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+    2442        9413 :       vec3d = quat_eigen * vec3d;
+    2443             : 
+    2444        9413 :       point.x = uav_x + vec3d(0);
+    2445        9413 :       point.y = uav_y + vec3d(1);
+    2446        9413 :       point.z = uav_z + vec3d(2);
+    2447             : 
+    2448        9413 :       marker.points.push_back(point);
+    2449             : 
+    2450             :       //}
+    2451             : 
+    2452        9413 :       marker.scale.x = 0.05;
+    2453        9413 :       marker.scale.y = 0.05;
+    2454        9413 :       marker.scale.z = 0.05;
+    2455             : 
+    2456        9413 :       marker.color.a = 0.5;
+    2457        9413 :       marker.color.r = 0.0;
+    2458        9413 :       marker.color.g = 1.0;
+    2459        9413 :       marker.color.b = 0.0;
+    2460             : 
+    2461        9413 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2462             : 
+    2463        9413 :       msg_out.markers.push_back(marker);
+    2464             :     }
+    2465             : 
+    2466             :     //}
+    2467             : 
+    2468        9413 :     ph_disturbances_markers_.publish(msg_out);
+    2469             :   }
+    2470             : 
+    2471             :   // --------------------------------------------------------------
+    2472             :   // |               publish the current constraints              |
+    2473             :   // --------------------------------------------------------------
+    2474             : 
+    2475       17162 :   if (got_constraints_) {
+    2476             : 
+    2477       14546 :     auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+    2478             : 
+    2479       14546 :     mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+    2480             : 
+    2481       14546 :     ph_current_constraints_.publish(constraints);
+    2482             :   }
+    2483             : }
+    2484             : 
+    2485             : //}
+    2486             : 
+    2487             : /* //{ timerSafety() */
+    2488             : 
+    2489      313812 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+    2490             : 
+    2491      313812 :   mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+    2492             : 
+    2493      313813 :   if (!is_initialized_)
+    2494           0 :     return;
+    2495             : 
+    2496      627626 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+    2497      627626 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+    2498             : 
+    2499             :   // copy member variables
+    2500      313813 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2501      313813 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    2502      313813 :   auto [uav_state, uav_yaw]  = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+    2503      313813 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2504      313813 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2505             : 
+    2506      598954 :   if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+    2507      285141 :       active_tracker_idx == _null_tracker_idx_) {
+    2508      105842 :     return;
+    2509             :   }
+    2510             : 
+    2511      207971 :   if (odometry_switch_in_progress_) {
+    2512           5 :     ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+    2513           5 :     return;
+    2514             :   }
+    2515             : 
+    2516             :   // | ------------------------ timeouts ------------------------ |
+    2517             : 
+    2518      207966 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2519      207966 :     double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+    2520             : 
+    2521      207965 :     if (missing_for > _uav_state_max_missing_time_) {
+    2522           8 :       timeoutUavState(missing_for);
+    2523             :     }
+    2524             :   }
+    2525             : 
+    2526      207966 :   if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+    2527           0 :     double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+    2528             : 
+    2529           0 :     if (missing_for > _uav_state_max_missing_time_) {
+    2530           0 :       timeoutUavState(missing_for);
+    2531             :     }
+    2532             :   }
+    2533             : 
+    2534             :   // | -------------- eland and failsafe thresholds ------------- |
+    2535             : 
+    2536      207966 :   std::map<std::string, ControllerParams>::iterator it;
+    2537      207966 :   it = controllers_.find(_controller_names_.at(active_controller_idx));
+    2538             : 
+    2539      207966 :   _eland_threshold_               = it->second.eland_threshold;
+    2540      207966 :   _failsafe_threshold_            = it->second.failsafe_threshold;
+    2541      207965 :   _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+    2542             : 
+    2543             :   // | --------- calculate control errors and tilt angle -------- |
+    2544             : 
+    2545             :   // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+    2546             :   // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+    2547      207966 :   if (!last_tracker_cmd || !last_control_output.control_output) {
+    2548         170 :     return;
+    2549             :   }
+    2550             : 
+    2551             :   {
+    2552      207795 :     std::scoped_lock lock(mutex_attitude_error_);
+    2553             : 
+    2554      207796 :     tilt_error_ = 0;
+    2555      207796 :     yaw_error_  = 0;
+    2556             :   }
+    2557             : 
+    2558             :   {
+    2559      207796 :     Eigen::Vector3d position_error = Eigen::Vector3d::Zero();
+    2560             : 
+    2561      207796 :     bool position_error_set = false;
+    2562             : 
+    2563      207796 :     if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2564             : 
+    2565      206773 :       position_error(0) = last_tracker_cmd->position.x - uav_state.pose.position.x;
+    2566      206772 :       position_error(1) = last_tracker_cmd->position.y - uav_state.pose.position.y;
+    2567             : 
+    2568      206773 :       position_error_set = true;
+    2569             :     }
+    2570             : 
+    2571      207796 :     if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2572             : 
+    2573      206773 :       position_error(2) = last_tracker_cmd->position.z - uav_state.pose.position.z;
+    2574             : 
+    2575      206773 :       position_error_set = true;
+    2576             :     }
+    2577             : 
+    2578      207796 :     if (position_error_set) {
+    2579             : 
+    2580      206773 :       mrs_lib::set_mutexed(mutex_position_error_, {position_error}, position_error_);
+    2581             :     }
+    2582             :   }
+    2583             : 
+    2584             :   // rotate the drone's z axis
+    2585      207796 :   tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2586      207796 :   tf2::Vector3   uav_z_in_world      = uav_state_transform * tf2::Vector3(0, 0, 1);
+    2587             : 
+    2588             :   // calculate the angle between the drone's z axis and the world's z axis
+    2589      207796 :   double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+    2590             : 
+    2591             :   // | ------------ calculate the tilt and yaw error ------------ |
+    2592             : 
+    2593             :   // | --------------------- the tilt error --------------------- |
+    2594             : 
+    2595      207796 :   if (last_control_output.desired_orientation) {
+    2596             : 
+    2597             :     // calculate the desired drone's z axis in the world frame
+    2598      182462 :     tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+    2599      182462 :     tf2::Vector3   uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+    2600             : 
+    2601             :     {
+    2602      182462 :       std::scoped_lock lock(mutex_attitude_error_);
+    2603             : 
+    2604             :       // calculate the angle between the drone's z axis and the world's z axis
+    2605      182462 :       tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+    2606             : 
+    2607             :       // calculate the yaw error
+    2608      182462 :       double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+    2609      182462 :       yaw_error_     = fabs(radians::diff(cmd_yaw, uav_yaw));
+    2610             :     }
+    2611             :   }
+    2612             : 
+    2613      207796 :   auto position_error          = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2614      207796 :   auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+    2615             : 
+    2616             :   // --------------------------------------------------------------
+    2617             :   // |   activate the failsafe controller in case of large error  |
+    2618             :   // --------------------------------------------------------------
+    2619             : 
+    2620      207796 :   if (position_error) {
+    2621             : 
+    2622      206773 :     if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+    2623             : 
+    2624          14 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2625             : 
+    2626          14 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2627             : 
+    2628           1 :         if (!failsafe_triggered_) {
+    2629             : 
+    2630           1 :           ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+    2631             :                     _failsafe_threshold_, position_error.value()(0), position_error.value()(1), position_error.value()(2));
+    2632             : 
+    2633           1 :           failsafe();
+    2634             :         }
+    2635             :       }
+    2636             :     }
+    2637             :   }
+    2638             : 
+    2639             :   // --------------------------------------------------------------
+    2640             :   // |     activate emergency land in case of large innovation    |
+    2641             :   // --------------------------------------------------------------
+    2642             : 
+    2643      207796 :   if (_odometry_innovation_check_enabled_) {
+    2644             : 
+    2645      207796 :     std::optional<nav_msgs::Odometry::ConstPtr> innovation;
+    2646             : 
+    2647      207796 :     if (sh_odometry_innovation_.hasMsg()) {
+    2648      207796 :       innovation = {sh_odometry_innovation_.getMsg()};
+    2649             :     } else {
+    2650           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: missing estimator innnovation but the innovation check is enabled!");
+    2651             :     }
+    2652             : 
+    2653      207796 :     if (innovation) {
+    2654             : 
+    2655      207796 :       auto [x, y, z] = mrs_lib::getPosition(innovation.value());
+    2656             : 
+    2657      207794 :       double heading = 0;
+    2658             :       try {
+    2659      207794 :         heading = mrs_lib::getHeading(innovation.value());
+    2660             :       }
+    2661           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    2662           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+    2663             :       }
+    2664             : 
+    2665      207796 :       double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+    2666             : 
+    2667      207795 :       if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+    2668             : 
+    2669           0 :         auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2670             : 
+    2671           0 :         if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2672             : 
+    2673           0 :           if (!failsafe_triggered_ && !eland_triggered_) {
+    2674             : 
+    2675           0 :             ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+    2676             :                       last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+    2677             : 
+    2678           0 :             eland();
+    2679             :           }
+    2680             :         }
+    2681             :       }
+    2682             :     }
+    2683             :   }
+    2684             : 
+    2685             :   // --------------------------------------------------------------
+    2686             :   // |   activate emergency land in case of medium control error  |
+    2687             :   // --------------------------------------------------------------
+    2688             : 
+    2689             :   // | ------------------- tilt control error ------------------- |
+    2690             : 
+    2691      207796 :   if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+    2692             : 
+    2693           0 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2694             : 
+    2695           0 :     if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2696             : 
+    2697           0 :       if (!failsafe_triggered_ && !eland_triggered_) {
+    2698             : 
+    2699           0 :         ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+    2700             :                   (180.0 / M_PI) * _tilt_limit_eland_);
+    2701             : 
+    2702           0 :         eland();
+    2703             :       }
+    2704             :     }
+    2705             :   }
+    2706             : 
+    2707             :   // | ----------------- position control error ----------------- |
+    2708             : 
+    2709      207796 :   if (position_error) {
+    2710             : 
+    2711      206772 :     double error_size = position_error->norm();
+    2712             : 
+    2713      206772 :     if (error_size > _eland_threshold_ / 2.0) {
+    2714             : 
+    2715         812 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2716             : 
+    2717         812 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2718             : 
+    2719         595 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2720             : 
+    2721          76 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+    2722             : 
+    2723          76 :           ungripSrv();
+    2724             :         }
+    2725             :       }
+    2726             :     }
+    2727             : 
+    2728      206772 :     if (error_size > _eland_threshold_) {
+    2729             : 
+    2730         424 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2731             : 
+    2732         424 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2733             : 
+    2734         241 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2735             : 
+    2736           4 :           ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+    2737             :                     position_error.value()(0), position_error.value()(1), position_error.value()(2));
+    2738             : 
+    2739           4 :           eland();
+    2740             :         }
+    2741             :       }
+    2742             :     }
+    2743             :   }
+    2744             : 
+    2745             :   // | -------------------- yaw control error ------------------- |
+    2746             :   // do not have to mutex the yaw_error_ here since I am filling it in this function
+    2747             : 
+    2748      207795 :   if (_yaw_error_eland_enabled_ && yaw_error) {
+    2749             : 
+    2750      207795 :     if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+    2751             : 
+    2752           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2753             : 
+    2754           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2755             : 
+    2756           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2757             : 
+    2758           0 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2759             :                              (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+    2760             : 
+    2761           0 :           ungripSrv();
+    2762             :         }
+    2763             :       }
+    2764             :     }
+    2765             : 
+    2766      207794 :     if (yaw_error.value() > _yaw_error_eland_) {
+    2767             : 
+    2768           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2769             : 
+    2770           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2771             : 
+    2772           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2773             : 
+    2774           0 :           ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2775             :                     (180.0 / M_PI) * _yaw_error_eland_);
+    2776             : 
+    2777           0 :           eland();
+    2778             :         }
+    2779             :       }
+    2780             :     }
+    2781             :   }
+    2782             : 
+    2783             :   // --------------------------------------------------------------
+    2784             :   // |      disarm the drone when the tilt exceeds the limit      |
+    2785             :   // --------------------------------------------------------------
+    2786      207794 :   if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+    2787             : 
+    2788           0 :     ROS_ERROR("[ControlManager]: tilt angle too large, disarming: tilt angle=%.2f/%.2f deg", (180.0 / M_PI) * tilt_angle, (180.0 / M_PI) * _tilt_limit_disarm_);
+    2789             : 
+    2790           0 :     arming(false);
+    2791             :   }
+    2792             : 
+    2793             :   // --------------------------------------------------------------
+    2794             :   // |     disarm the drone when tilt error exceeds the limit     |
+    2795             :   // --------------------------------------------------------------
+    2796             : 
+    2797      207794 :   if (_tilt_error_disarm_enabled_ && tilt_error) {
+    2798             : 
+    2799      207795 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2800             : 
+    2801             :     // the time from the last controller/tracker switch
+    2802             :     // fyi: we should not
+    2803      207796 :     double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+    2804             : 
+    2805             :     // if the tile error is over the threshold
+    2806             :     // && we are not ramping up during takeoff
+    2807      207796 :     if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+    2808             : 
+    2809             :       // only account for the error if some time passed from the last tracker/controller switch
+    2810           0 :       if (time_from_ctrl_tracker_switch > 1.0) {
+    2811             : 
+    2812             :         // if the threshold was not exceeded before
+    2813           0 :         if (!tilt_error_disarm_over_thr_) {
+    2814             : 
+    2815           0 :           tilt_error_disarm_over_thr_ = true;
+    2816           0 :           tilt_error_disarm_time_     = ros::Time::now();
+    2817             : 
+    2818           0 :           ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+    2819             :                    (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+    2820             : 
+    2821             :           // if it was exceeded before, just keep it
+    2822             :         } else {
+    2823             : 
+    2824           0 :           ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+    2825             :                             (ros::Time::now() - tilt_error_disarm_time_).toSec());
+    2826             :         }
+    2827             : 
+    2828             :         // if the tile error is bad, but the controller just switched,
+    2829             :         // don't think its bad anymore
+    2830             :       } else {
+    2831             : 
+    2832           0 :         tilt_error_disarm_over_thr_ = false;
+    2833           0 :         tilt_error_disarm_time_     = ros::Time::now();
+    2834             :       }
+    2835             : 
+    2836             :       // if the tilt error is fine
+    2837             :     } else {
+    2838             : 
+    2839             :       // make it fine
+    2840      207795 :       tilt_error_disarm_over_thr_ = false;
+    2841      207795 :       tilt_error_disarm_time_     = ros::Time::now();
+    2842             :     }
+    2843             : 
+    2844             :     // calculate the time over the threshold
+    2845      207796 :     double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+    2846             : 
+    2847             :     // if the tot exceeds the limit (and if we are actually over the threshold)
+    2848      207796 :     if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+    2849             : 
+    2850           0 :       bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+    2851             : 
+    2852             :       // only when flying and not in failsafe
+    2853           0 :       if (is_flying) {
+    2854             : 
+    2855           0 :         ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+    2856             : 
+    2857           0 :         toggleOutput(false);
+    2858           0 :         arming(false);
+    2859             :       }
+    2860             :     }
+    2861             :   }
+    2862             : 
+    2863             :   // | --------- dropping out of OFFBOARD in mid flight --------- |
+    2864             : 
+    2865             :   // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+    2866      207796 :   if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+    2867             : 
+    2868           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+    2869             : 
+    2870           0 :     toggleOutput(false);
+    2871             :   }
+    2872             : }  // namespace control_manager
+    2873             : 
+    2874             : //}
+    2875             : 
+    2876             : /* //{ timerEland() */
+    2877             : 
+    2878         496 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+    2879             : 
+    2880         496 :   if (!is_initialized_)
+    2881         129 :     return;
+    2882             : 
+    2883         992 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+    2884         992 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+    2885             : 
+    2886             :   // copy member variables
+    2887         496 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2888             : 
+    2889         496 :   if (!last_control_output.control_output) {
+    2890           0 :     return;
+    2891             :   }
+    2892             : 
+    2893         496 :   auto throttle = extractThrottle(last_control_output);
+    2894             : 
+    2895         496 :   if (!throttle) {
+    2896         129 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality");
+    2897         129 :     return;
+    2898             :   }
+    2899             : 
+    2900         367 :   if (current_state_landing_ == IDLE_STATE) {
+    2901             : 
+    2902           0 :     return;
+    2903             : 
+    2904         367 :   } else if (current_state_landing_ == LANDING_STATE) {
+    2905             : 
+    2906             :     // --------------------------------------------------------------
+    2907             :     // |                            TODO                            |
+    2908             :     // This section needs work. The throttle landing detection      |
+    2909             :     // mechanism should be extracted and other mechanisms, such     |
+    2910             :     // as velocity-based detection should be used for high          |
+    2911             :     // modalities                                                   |
+    2912             :     // --------------------------------------------------------------
+    2913             : 
+    2914         367 :     if (!last_control_output.control_output) {
+    2915           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+    2916           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+    2917           0 :       return;
+    2918             :     }
+    2919             : 
+    2920             :     // recalculate the mass based on the throttle
+    2921         367 :     throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2922         367 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2923             : 
+    2924             :     // condition for automatic motor turn off
+    2925         367 :     if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+    2926          88 :       if (!throttle_under_threshold_) {
+    2927             : 
+    2928           4 :         throttle_mass_estimate_first_time_ = ros::Time::now();
+    2929           4 :         throttle_under_threshold_          = true;
+    2930             :       }
+    2931             : 
+    2932          88 :       ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2933             : 
+    2934             :     } else {
+    2935         279 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2936         279 :       throttle_under_threshold_          = false;
+    2937             :     }
+    2938             : 
+    2939         367 :     if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    2940             :       // enable callbacks? ... NO
+    2941             : 
+    2942           4 :       ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+    2943           4 :       toggleOutput(false);
+    2944             : 
+    2945             :       // disarm the drone
+    2946           4 :       if (_eland_disarm_enabled_) {
+    2947             : 
+    2948           4 :         ROS_INFO("[ControlManager]: calling for disarm");
+    2949           4 :         arming(false);
+    2950             :       }
+    2951             : 
+    2952           4 :       changeLandingState(IDLE_STATE);
+    2953             : 
+    2954           4 :       ROS_WARN("[ControlManager]: emergency landing finished");
+    2955             : 
+    2956           4 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    2957           4 :       timer_eland_.stop();
+    2958           4 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    2959             : 
+    2960             :       // we should NOT set eland_triggered_=true
+    2961             :     }
+    2962             :   }
+    2963             : }
+    2964             : 
+    2965             : //}
+    2966             : 
+    2967             : /* //{ timerFailsafe() */
+    2968             : 
+    2969        9721 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+    2970             : 
+    2971        9721 :   if (!is_initialized_) {
+    2972           0 :     return;
+    2973             :   }
+    2974             : 
+    2975        9721 :   ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+    2976             : 
+    2977       19442 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+    2978       19442 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+    2979             : 
+    2980             :   // copy member variables
+    2981        9721 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2982             : 
+    2983        9721 :   updateControllers(uav_state);
+    2984             : 
+    2985        9721 :   publish();
+    2986             : 
+    2987        9721 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2988             : 
+    2989        9721 :   if (!last_control_output.control_output) {
+    2990           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+    2991           0 :     return;
+    2992             :   }
+    2993             : 
+    2994        9721 :   auto throttle = extractThrottle(last_control_output);
+    2995             : 
+    2996        9721 :   if (!throttle) {
+    2997           0 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+    2998           0 :     return;
+    2999             :   }
+    3000             : 
+    3001             :   // --------------------------------------------------------------
+    3002             :   // |                            TODO                            |
+    3003             :   // This section needs work. The throttle landing detection      |
+    3004             :   // mechanism should be extracted and other mechanisms, such     |
+    3005             :   // as velocity-based detection should be used for high          |
+    3006             :   // modalities                                                   |
+    3007             :   // --------------------------------------------------------------
+    3008             : 
+    3009        9721 :   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    3010        9721 :   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    3011             : 
+    3012             :   // condition for automatic motor turn off
+    3013        9721 :   if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+    3014             : 
+    3015        1414 :     if (!throttle_under_threshold_) {
+    3016             : 
+    3017           7 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    3018           7 :       throttle_under_threshold_          = true;
+    3019             :     }
+    3020             : 
+    3021        1414 :     ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    3022             : 
+    3023             :   } else {
+    3024             : 
+    3025        8307 :     throttle_mass_estimate_first_time_ = ros::Time::now();
+    3026        8307 :     throttle_under_threshold_          = false;
+    3027             :   }
+    3028             : 
+    3029             :   // condition for automatic motor turn off
+    3030        9721 :   if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    3031             : 
+    3032           7 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+    3033             : 
+    3034           7 :     arming(false);
+    3035             :   }
+    3036             : }
+    3037             : 
+    3038             : //}
+    3039             : 
+    3040             : /* //{ timerJoystick() */
+    3041             : 
+    3042       60884 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+    3043             : 
+    3044       60884 :   if (!is_initialized_) {
+    3045           0 :     return;
+    3046             :   }
+    3047             : 
+    3048      182652 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+    3049      182652 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3050             : 
+    3051      121768 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3052             : 
+    3053             :   // if start was pressed and held for > 3.0 s
+    3054       60884 :   if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+    3055             : 
+    3056           0 :     joystick_start_press_time_ = ros::Time(0);
+    3057             : 
+    3058           0 :     ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+    3059             :              _joystick_controller_name_.c_str());
+    3060             : 
+    3061           0 :     joystick_start_pressed_ = false;
+    3062             : 
+    3063           0 :     switchTracker(_joystick_tracker_name_);
+    3064           0 :     switchController(_joystick_controller_name_);
+    3065             :   }
+    3066             : 
+    3067             :   // if RT+LT were pressed and held for > 0.1 s
+    3068       60884 :   if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+    3069             : 
+    3070           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3071             : 
+    3072           0 :     ROS_INFO("[ControlManager]: activating failsafe by joystick");
+    3073             : 
+    3074           0 :     joystick_failsafe_pressed_ = false;
+    3075             : 
+    3076           0 :     failsafe();
+    3077             :   }
+    3078             : 
+    3079             :   // if joypads were pressed and held for > 0.1 s
+    3080       60884 :   if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+    3081             : 
+    3082           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3083             : 
+    3084           0 :     ROS_INFO("[ControlManager]: activating eland by joystick");
+    3085             : 
+    3086           0 :     joystick_failsafe_pressed_ = false;
+    3087             : 
+    3088           0 :     eland();
+    3089             :   }
+    3090             : 
+    3091             :   // if back was pressed and held for > 0.1 s
+    3092       60884 :   if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+    3093             : 
+    3094           0 :     joystick_back_press_time_ = ros::Time(0);
+    3095             : 
+    3096             :     // activate/deactivate the joystick goto functionality
+    3097           0 :     joystick_goto_enabled_ = !joystick_goto_enabled_;
+    3098             : 
+    3099           0 :     ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+    3100             :   }
+    3101             : 
+    3102             :   // if the GOTO functionality is enabled...
+    3103       60884 :   if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+    3104             : 
+    3105           0 :     auto joystick_data = sh_joystick_.getMsg();
+    3106             : 
+    3107             :     // create the reference
+    3108             : 
+    3109           0 :     mrs_msgs::Vec4::Request request;
+    3110             : 
+    3111           0 :     if (fabs(joystick_data->axes.at(_channel_pitch_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_roll_)) >= 0.05 ||
+    3112           0 :         fabs(joystick_data->axes.at(_channel_heading_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_throttle_)) >= 0.05) {
+    3113             : 
+    3114           0 :       if (_joystick_mode_ == 0) {
+    3115             : 
+    3116           0 :         request.goal.at(REF_X)       = _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * _joystick_carrot_distance_;
+    3117           0 :         request.goal.at(REF_Y)       = _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * _joystick_carrot_distance_;
+    3118           0 :         request.goal.at(REF_Z)       = _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_);
+    3119           0 :         request.goal.at(REF_HEADING) = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+    3120             : 
+    3121           0 :         mrs_msgs::Vec4::Response response;
+    3122             : 
+    3123           0 :         callbackGotoFcu(request, response);
+    3124             : 
+    3125           0 :       } else if (_joystick_mode_ == 1) {
+    3126             : 
+    3127           0 :         mrs_msgs::TrajectoryReference trajectory;
+    3128             : 
+    3129           0 :         double dt = 0.2;
+    3130             : 
+    3131           0 :         trajectory.fly_now         = true;
+    3132           0 :         trajectory.header.frame_id = "fcu_untilted";
+    3133           0 :         trajectory.use_heading     = true;
+    3134           0 :         trajectory.dt              = dt;
+    3135             : 
+    3136           0 :         mrs_msgs::Reference point;
+    3137           0 :         point.position.x = 0;
+    3138           0 :         point.position.y = 0;
+    3139           0 :         point.position.z = 0;
+    3140           0 :         point.heading    = 0;
+    3141             : 
+    3142           0 :         trajectory.points.push_back(point);
+    3143             : 
+    3144           0 :         double speed = 1.0;
+    3145             : 
+    3146           0 :         for (int i = 0; i < 50; i++) {
+    3147             : 
+    3148           0 :           point.position.x += _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * (speed * dt);
+    3149           0 :           point.position.y += _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * (speed * dt);
+    3150           0 :           point.position.z += _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_) * (speed * dt);
+    3151           0 :           point.heading = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+    3152             : 
+    3153           0 :           trajectory.points.push_back(point);
+    3154             :         }
+    3155             : 
+    3156           0 :         setTrajectoryReference(trajectory);
+    3157             :       }
+    3158             :     }
+    3159             :   }
+    3160             : 
+    3161       60884 :   if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+    3162             : 
+    3163             :     // create the reference
+    3164        1390 :     mrs_msgs::VelocityReferenceStampedSrv::Request request;
+    3165             : 
+    3166         695 :     double des_x       = 0;
+    3167         695 :     double des_y       = 0;
+    3168         695 :     double des_z       = 0;
+    3169         695 :     double des_heading = 0;
+    3170             : 
+    3171         695 :     bool nothing_to_do = true;
+    3172             : 
+    3173             :     // copy member variables
+    3174        1390 :     mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+    3175             : 
+    3176         695 :     if (rc_channels->channels.size() < 4) {
+    3177             : 
+    3178           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+    3179             :                          int(rc_channels->channels.size()));
+    3180           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+    3181             : 
+    3182             :     } else {
+    3183             : 
+    3184         695 :       double tmp_x       = RCChannelToRange(rc_channels->channels.at(_rc_channel_pitch_), _rc_horizontal_speed_, 0.1);
+    3185         695 :       double tmp_y       = -RCChannelToRange(rc_channels->channels.at(_rc_channel_roll_), _rc_horizontal_speed_, 0.1);
+    3186         695 :       double tmp_z       = RCChannelToRange(rc_channels->channels.at(_rc_channel_throttle_), _rc_vertical_speed_, 0.3);
+    3187         695 :       double tmp_heading = -RCChannelToRange(rc_channels->channels.at(_rc_channel_heading_), _rc_heading_rate_, 0.1);
+    3188             : 
+    3189         695 :       if (abs(tmp_x) > 1e-3) {
+    3190         245 :         des_x         = tmp_x;
+    3191         245 :         nothing_to_do = false;
+    3192             :       }
+    3193             : 
+    3194         695 :       if (abs(tmp_y) > 1e-3) {
+    3195         115 :         des_y         = tmp_y;
+    3196         115 :         nothing_to_do = false;
+    3197             :       }
+    3198             : 
+    3199         695 :       if (abs(tmp_z) > 1e-3) {
+    3200         223 :         des_z         = tmp_z;
+    3201         223 :         nothing_to_do = false;
+    3202             :       }
+    3203             : 
+    3204         695 :       if (abs(tmp_heading) > 1e-3) {
+    3205          48 :         des_heading   = tmp_heading;
+    3206          48 :         nothing_to_do = false;
+    3207             :       }
+    3208             :     }
+    3209             : 
+    3210         695 :     if (!nothing_to_do) {
+    3211             : 
+    3212         631 :       request.reference.header.frame_id = "fcu_untilted";
+    3213             : 
+    3214         631 :       request.reference.reference.use_heading_rate = true;
+    3215             : 
+    3216         631 :       request.reference.reference.velocity.x   = des_x;
+    3217         631 :       request.reference.reference.velocity.y   = des_y;
+    3218         631 :       request.reference.reference.velocity.z   = des_z;
+    3219         631 :       request.reference.reference.heading_rate = des_heading;
+    3220             : 
+    3221        1262 :       mrs_msgs::VelocityReferenceStampedSrv::Response response;
+    3222             : 
+    3223             :       // disable callbacks of all trackers
+    3224         631 :       std_srvs::SetBoolRequest req_enable_callbacks;
+    3225             : 
+    3226             :       // enable the callbacks for the active tracker
+    3227         631 :       req_enable_callbacks.data = true;
+    3228             :       {
+    3229         631 :         std::scoped_lock lock(mutex_tracker_list_);
+    3230             : 
+    3231         631 :         tracker_list_.at(active_tracker_idx_)
+    3232         631 :             ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3233             :       }
+    3234             : 
+    3235         631 :       callbacks_enabled_ = true;
+    3236             : 
+    3237         631 :       callbackVelocityReferenceService(request, response);
+    3238             : 
+    3239         631 :       callbacks_enabled_ = false;
+    3240             : 
+    3241         631 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: goto by RC with speed x=%.2f, y=%.2f, z=%.2f, heading_rate=%.2f", des_x, des_y, des_z, des_heading);
+    3242             : 
+    3243             :       // disable the callbacks back again
+    3244         631 :       req_enable_callbacks.data = false;
+    3245             :       {
+    3246         631 :         std::scoped_lock lock(mutex_tracker_list_);
+    3247             : 
+    3248         631 :         tracker_list_.at(active_tracker_idx_)
+    3249         631 :             ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3250             :       }
+    3251             :     }
+    3252             :   }
+    3253             : }
+    3254             : 
+    3255             : //}
+    3256             : 
+    3257             : /* //{ timerBumper() */
+    3258             : 
+    3259        2232 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+    3260             : 
+    3261        2232 :   if (!is_initialized_)
+    3262        1972 :     return;
+    3263             : 
+    3264        4464 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+    3265        4464 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+    3266             : 
+    3267             :   // | ---------------------- preconditions --------------------- |
+    3268             : 
+    3269        2232 :   if (!sh_bumper_.hasMsg()) {
+    3270        1968 :     return;
+    3271             :   }
+    3272             : 
+    3273         264 :   if (!bumper_enabled_) {
+    3274           0 :     return;
+    3275             :   }
+    3276             : 
+    3277         264 :   if (!isFlyingNormally() && !bumper_repulsing_) {
+    3278           4 :     return;
+    3279             :   }
+    3280             : 
+    3281         260 :   if (!got_uav_state_) {
+    3282           0 :     return;
+    3283             :   }
+    3284             : 
+    3285         260 :   if (sh_bumper_.hasMsg() && (ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+    3286             : 
+    3287           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: obstacle bumper is missing messages although we got them before");
+    3288             : 
+    3289           0 :     return;
+    3290             :   }
+    3291             : 
+    3292         260 :   timer_bumper_.setPeriod(ros::Duration(1.0 / _bumper_timer_rate_));
+    3293             : 
+    3294             :   // | ------------------ call the bumper logic ----------------- |
+    3295             : 
+    3296         260 :   bumperPushFromObstacle();
+    3297             : }
+    3298             : 
+    3299             : //}
+    3300             : 
+    3301             : /* //{ timerPirouette() */
+    3302             : 
+    3303           0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+    3304             : 
+    3305           0 :   if (!is_initialized_)
+    3306           0 :     return;
+    3307             : 
+    3308           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+    3309           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+    3310             : 
+    3311           0 :   pirouette_iterator_++;
+    3312             : 
+    3313           0 :   double pirouette_duration  = (2 * M_PI) / _pirouette_speed_;
+    3314           0 :   double pirouette_n_steps   = pirouette_duration * _pirouette_timer_rate_;
+    3315           0 :   double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+    3316             : 
+    3317           0 :   if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+    3318             : 
+    3319           0 :     _pirouette_enabled_ = false;
+    3320           0 :     timer_pirouette_.stop();
+    3321             : 
+    3322           0 :     setCallbacks(true);
+    3323             : 
+    3324           0 :     return;
+    3325             :   }
+    3326             : 
+    3327             :   // set the reference
+    3328           0 :   mrs_msgs::ReferenceStamped reference_request;
+    3329             : 
+    3330           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3331             : 
+    3332           0 :   reference_request.header.frame_id      = "";
+    3333           0 :   reference_request.header.stamp         = ros::Time(0);
+    3334           0 :   reference_request.reference.position.x = last_tracker_cmd->position.x;
+    3335           0 :   reference_request.reference.position.y = last_tracker_cmd->position.y;
+    3336           0 :   reference_request.reference.position.z = last_tracker_cmd->position.z;
+    3337           0 :   reference_request.reference.heading    = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+    3338             : 
+    3339             :   // enable the callbacks for the active tracker
+    3340             :   {
+    3341           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3342             : 
+    3343           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3344           0 :     req_enable_callbacks.data = true;
+    3345             : 
+    3346           0 :     tracker_list_.at(active_tracker_idx_)
+    3347           0 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3348             : 
+    3349           0 :     callbacks_enabled_ = true;
+    3350             :   }
+    3351             : 
+    3352           0 :   setReference(reference_request);
+    3353             : 
+    3354             :   {
+    3355           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3356             : 
+    3357             :     // disable the callbacks for the active tracker
+    3358           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3359           0 :     req_enable_callbacks.data = false;
+    3360             : 
+    3361           0 :     tracker_list_.at(active_tracker_idx_)
+    3362           0 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3363             : 
+    3364           0 :     callbacks_enabled_ = false;
+    3365             :   }
+    3366             : }
+    3367             : 
+    3368             : //}
+    3369             : 
+    3370             : // --------------------------------------------------------------
+    3371             : // |                           asyncs                           |
+    3372             : // --------------------------------------------------------------
+    3373             : 
+    3374             : /* asyncControl() //{ */
+    3375             : 
+    3376      136502 : void ControlManager::asyncControl(void) {
+    3377             : 
+    3378      136502 :   if (!is_initialized_)
+    3379           0 :     return;
+    3380             : 
+    3381      273004 :   mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+    3382             : 
+    3383      409506 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("asyncControl");
+    3384      409506 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+    3385             : 
+    3386             :   // copy member variables
+    3387      273004 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3388      136502 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    3389             : 
+    3390      136502 :   if (!failsafe_triggered_) {  // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+    3391             : 
+    3392             :     // run the safety timer
+    3393             :     // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+    3394      127143 :     while (running_safety_timer_) {
+    3395             : 
+    3396        1097 :       ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3397        1097 :       ros::Duration wait(0.001);
+    3398        1097 :       wait.sleep();
+    3399             : 
+    3400        1097 :       if (!running_safety_timer_) {
+    3401        1088 :         ROS_DEBUG("[ControlManager]: safety timer finished");
+    3402        1088 :         break;
+    3403             :       }
+    3404             :     }
+    3405             : 
+    3406      127134 :     ros::TimerEvent safety_timer_event;
+    3407      127134 :     timerSafety(safety_timer_event);
+    3408             : 
+    3409      127134 :     updateTrackers();
+    3410             : 
+    3411      127134 :     updateControllers(uav_state);
+    3412             : 
+    3413      127134 :     if (got_constraints_) {
+    3414             : 
+    3415             :       // update the constraints to trackers, if need to
+    3416      126478 :       auto enforce = enforceControllersConstraints(current_constraints);
+    3417             : 
+    3418      126478 :       if (enforce && !constraints_being_enforced_) {
+    3419             : 
+    3420          70 :         setConstraintsToTrackers(enforce.value());
+    3421          70 :         mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+    3422             : 
+    3423          70 :         constraints_being_enforced_ = true;
+    3424             : 
+    3425          70 :         auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3426             : 
+    3427          70 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+    3428             :                           _controller_names_.at(active_controller_idx).c_str());
+    3429             : 
+    3430      126408 :       } else if (!enforce && constraints_being_enforced_) {
+    3431             : 
+    3432          64 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+    3433             : 
+    3434          64 :         constraints_being_enforced_ = false;
+    3435             : 
+    3436          64 :         mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+    3437             : 
+    3438          64 :         setConstraintsToTrackers(current_constraints);
+    3439             :       }
+    3440             :     }
+    3441             : 
+    3442      127134 :     publish();
+    3443             :   }
+    3444             : 
+    3445             :   // if odometry switch happened, we finish it here and turn the safety timer back on
+    3446      136502 :   if (odometry_switch_in_progress_) {
+    3447             : 
+    3448           5 :     ROS_DEBUG("[ControlManager]: starting safety timer");
+    3449           5 :     timer_safety_.start();
+    3450           5 :     ROS_DEBUG("[ControlManager]: safety timer started");
+    3451           5 :     odometry_switch_in_progress_ = false;
+    3452             : 
+    3453             :     {
+    3454          10 :       std::scoped_lock lock(mutex_uav_state_);
+    3455             : 
+    3456           5 :       ROS_INFO("[ControlManager]: odometry after switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+    3457             :                uav_state.pose.position.z, uav_heading_);
+    3458             :     }
+    3459             :   }
+    3460             : }
+    3461             : 
+    3462             : //}
+    3463             : 
+    3464             : // --------------------------------------------------------------
+    3465             : // |                          callbacks                         |
+    3466             : // --------------------------------------------------------------
+    3467             : 
+    3468             : // | --------------------- topic callbacks -------------------- |
+    3469             : 
+    3470             : /* //{ callbackOdometry() */
+    3471             : 
+    3472           0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    3473             : 
+    3474           0 :   if (!is_initialized_)
+    3475           0 :     return;
+    3476             : 
+    3477           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackOdometry");
+    3478           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+    3479             : 
+    3480           0 :   nav_msgs::OdometryConstPtr odom = msg;
+    3481             : 
+    3482             :   // | ------------------ check for time stamp ------------------ |
+    3483             : 
+    3484             :   {
+    3485           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3486             : 
+    3487           0 :     if (uav_state_.header.stamp == odom->header.stamp) {
+    3488           0 :       return;
+    3489             :     }
+    3490             :   }
+    3491             : 
+    3492             :   // | --------------------- check for nans --------------------- |
+    3493             : 
+    3494           0 :   if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+    3495           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+    3496           0 :     return;
+    3497             :   }
+    3498             : 
+    3499             :   // | ---------------------- frame switch ---------------------- |
+    3500             : 
+    3501             :   /* Odometry frame switch //{ */
+    3502             : 
+    3503             :   // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+    3504             : 
+    3505           0 :   mrs_msgs::UavState uav_state_odom;
+    3506             : 
+    3507           0 :   uav_state_odom.header   = odom->header;
+    3508           0 :   uav_state_odom.pose     = odom->pose.pose;
+    3509           0 :   uav_state_odom.velocity = odom->twist.twist;
+    3510             : 
+    3511             :   // | ----- check for change in odometry frame of reference ---- |
+    3512             : 
+    3513           0 :   if (got_uav_state_) {
+    3514             : 
+    3515           0 :     if (odom->header.frame_id != uav_state_.header.frame_id) {
+    3516             : 
+    3517           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3518             :       {
+    3519           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3520             : 
+    3521           0 :         ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+    3522             :                  uav_state_.pose.position.z, uav_heading_);
+    3523             :       }
+    3524             : 
+    3525           0 :       odometry_switch_in_progress_ = true;
+    3526             : 
+    3527             :       // we have to stop safety timer, otherwise it will interfere
+    3528           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3529           0 :       timer_safety_.stop();
+    3530           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3531             : 
+    3532             :       // wait for the safety timer to stop if its running
+    3533           0 :       while (running_safety_timer_) {
+    3534             : 
+    3535           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3536           0 :         ros::Duration wait(0.001);
+    3537           0 :         wait.sleep();
+    3538             : 
+    3539           0 :         if (!running_safety_timer_) {
+    3540           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3541           0 :           break;
+    3542             :         }
+    3543             :       }
+    3544             : 
+    3545             :       // we have to also for the oneshot control timer to finish
+    3546           0 :       while (running_async_control_) {
+    3547             : 
+    3548           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3549           0 :         ros::Duration wait(0.001);
+    3550           0 :         wait.sleep();
+    3551             : 
+    3552           0 :         if (!running_async_control_) {
+    3553           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3554           0 :           break;
+    3555             :         }
+    3556             :       }
+    3557             : 
+    3558             :       {
+    3559           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3560             : 
+    3561           0 :         tracker_list_.at(active_tracker_idx_)->switchOdometrySource(uav_state_odom);
+    3562           0 :         controller_list_.at(active_controller_idx_)->switchOdometrySource(uav_state_odom);
+    3563             :       }
+    3564             :     }
+    3565             :   }
+    3566             : 
+    3567             :   //}
+    3568             : 
+    3569             :   // | ----------- copy the odometry to the uav_state ----------- |
+    3570             : 
+    3571             :   {
+    3572           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3573             : 
+    3574           0 :     previous_uav_state_ = uav_state_;
+    3575             : 
+    3576           0 :     uav_state_ = mrs_msgs::UavState();
+    3577             : 
+    3578           0 :     uav_state_.header           = odom->header;
+    3579           0 :     uav_state_.pose             = odom->pose.pose;
+    3580           0 :     uav_state_.velocity.angular = odom->twist.twist.angular;
+    3581             : 
+    3582             :     // transform the twist into the header's frame
+    3583             :     {
+    3584             :       // the velocity from the odometry
+    3585           0 :       geometry_msgs::Vector3Stamped speed_child_frame;
+    3586           0 :       speed_child_frame.header.frame_id = odom->child_frame_id;
+    3587           0 :       speed_child_frame.header.stamp    = odom->header.stamp;
+    3588           0 :       speed_child_frame.vector.x        = odom->twist.twist.linear.x;
+    3589           0 :       speed_child_frame.vector.y        = odom->twist.twist.linear.y;
+    3590           0 :       speed_child_frame.vector.z        = odom->twist.twist.linear.z;
+    3591             : 
+    3592           0 :       auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+    3593             : 
+    3594           0 :       if (res) {
+    3595           0 :         uav_state_.velocity.linear.x = res.value().vector.x;
+    3596           0 :         uav_state_.velocity.linear.y = res.value().vector.y;
+    3597           0 :         uav_state_.velocity.linear.z = res.value().vector.z;
+    3598             :       } else {
+    3599           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+    3600             :                            odom->header.frame_id.c_str());
+    3601           0 :         return;
+    3602             :       }
+    3603             :     }
+    3604             : 
+    3605             :     // calculate the euler angles
+    3606           0 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+    3607             : 
+    3608             :     try {
+    3609           0 :       uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+    3610             :     }
+    3611           0 :     catch (...) {
+    3612           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+    3613             :     }
+    3614             : 
+    3615           0 :     transformer_->setDefaultFrame(odom->header.frame_id);
+    3616             : 
+    3617           0 :     got_uav_state_ = true;
+    3618             :   }
+    3619             : 
+    3620             :   // run the control loop asynchronously in an OneShotTimer
+    3621             :   // but only if its not already running
+    3622           0 :   if (!running_async_control_) {
+    3623             : 
+    3624           0 :     running_async_control_ = true;
+    3625             : 
+    3626           0 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3627             :   }
+    3628             : }
+    3629             : 
+    3630             : //}
+    3631             : 
+    3632             : /* //{ callbackUavState() */
+    3633             : 
+    3634      158588 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    3635             : 
+    3636      158588 :   if (!is_initialized_)
+    3637       19998 :     return;
+    3638             : 
+    3639      317176 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackUavState");
+    3640      317176 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+    3641             : 
+    3642      158588 :   mrs_msgs::UavStateConstPtr uav_state = msg;
+    3643             : 
+    3644             :   // | ------------------ check for time stamp ------------------ |
+    3645             : 
+    3646             :   {
+    3647      158588 :     std::scoped_lock lock(mutex_uav_state_);
+    3648             : 
+    3649      158588 :     if (uav_state_.header.stamp == uav_state->header.stamp) {
+    3650       19998 :       return;
+    3651             :     }
+    3652             :   }
+    3653             : 
+    3654             :   // | --------------------- check for nans --------------------- |
+    3655             : 
+    3656      138590 :   if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+    3657           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+    3658           0 :     return;
+    3659             :   }
+    3660             : 
+    3661             :   // | -------------------- check for hiccups ------------------- |
+    3662             : 
+    3663             :   /* hickup detection //{ */
+    3664             : 
+    3665      138590 :   double alpha               = 0.99;
+    3666      138590 :   double alpha2              = 0.666;
+    3667      138590 :   double uav_state_count_lim = 1000;
+    3668             : 
+    3669      138590 :   double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+    3670             : 
+    3671             :   // belive only reasonable numbers
+    3672      138590 :   if (uav_state_dt <= 1.0) {
+    3673             : 
+    3674      138402 :     uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+    3675             : 
+    3676      138402 :     if (uav_state_count_ < uav_state_count_lim) {
+    3677       72852 :       uav_state_count_++;
+    3678             :     }
+    3679             :   }
+    3680             : 
+    3681      138590 :   if (uav_state_count_ == uav_state_count_lim) {
+    3682             : 
+    3683             :     /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+    3684             : 
+    3685       65606 :     if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+    3686             : 
+    3687       24189 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+    3688             : 
+    3689       41417 :     } else if (uav_state_avg_dt_ > 0.0001) {
+    3690             : 
+    3691       41417 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+    3692             :     }
+    3693             : 
+    3694       65606 :     if (uav_state_hiccup_factor_ > 3.141592653) {
+    3695             : 
+    3696             :       /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+    3697             : 
+    3698           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3699           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3700           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3701           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |            UAV_STATE has a large hiccup factor!            |");
+    3702           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           hint, hint: you are probably rosbagging          |");
+    3703           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           lot of data or publishing lot of large           |");
+    3704           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |          messages without mutual nodelet managers.         |");
+    3705           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3706           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3707           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3708             :     }
+    3709             :   }
+    3710             : 
+    3711             :   //}
+    3712             : 
+    3713             :   // | ---------------------- frame switch ---------------------- |
+    3714             : 
+    3715             :   /* frame switch //{ */
+    3716             : 
+    3717             :   // | ----- check for change in odometry frame of reference ---- |
+    3718             : 
+    3719      138590 :   if (got_uav_state_) {
+    3720             : 
+    3721      138496 :     if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+    3722             : 
+    3723           5 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3724             :       {
+    3725          10 :         std::scoped_lock lock(mutex_uav_state_);
+    3726             : 
+    3727           5 :         ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+    3728             :                  uav_state_.pose.position.z, uav_heading_);
+    3729             :       }
+    3730             : 
+    3731           5 :       odometry_switch_in_progress_ = true;
+    3732             : 
+    3733             :       // we have to stop safety timer, otherwise it will interfere
+    3734           5 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3735           5 :       timer_safety_.stop();
+    3736           5 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3737             : 
+    3738             :       // wait for the safety timer to stop if its running
+    3739           5 :       while (running_safety_timer_) {
+    3740             : 
+    3741           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3742           0 :         ros::Duration wait(0.001);
+    3743           0 :         wait.sleep();
+    3744             : 
+    3745           0 :         if (!running_safety_timer_) {
+    3746           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3747           0 :           break;
+    3748             :         }
+    3749             :       }
+    3750             : 
+    3751             :       // we have to also for the oneshot control timer to finish
+    3752           5 :       while (running_async_control_) {
+    3753             : 
+    3754           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3755           0 :         ros::Duration wait(0.001);
+    3756           0 :         wait.sleep();
+    3757             : 
+    3758           0 :         if (!running_async_control_) {
+    3759           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3760           0 :           break;
+    3761             :         }
+    3762             :       }
+    3763             : 
+    3764             :       {
+    3765          10 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3766             : 
+    3767           5 :         tracker_list_.at(active_tracker_idx_)->switchOdometrySource(*uav_state);
+    3768           5 :         controller_list_.at(active_controller_idx_)->switchOdometrySource(*uav_state);
+    3769             :       }
+    3770             :     }
+    3771             :   }
+    3772             : 
+    3773             :   //}
+    3774             : 
+    3775             :   // --------------------------------------------------------------
+    3776             :   // |           copy the UavState message for later use          |
+    3777             :   // --------------------------------------------------------------
+    3778             : 
+    3779             :   {
+    3780      138590 :     std::scoped_lock lock(mutex_uav_state_);
+    3781             : 
+    3782      138590 :     previous_uav_state_ = uav_state_;
+    3783             : 
+    3784      138590 :     uav_state_ = *uav_state;
+    3785             : 
+    3786      138590 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+    3787             : 
+    3788             :     try {
+    3789      138590 :       uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+    3790             :     }
+    3791           0 :     catch (...) {
+    3792           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+    3793             :     }
+    3794             : 
+    3795      138590 :     transformer_->setDefaultFrame(uav_state->header.frame_id);
+    3796             : 
+    3797      138590 :     got_uav_state_ = true;
+    3798             :   }
+    3799             : 
+    3800             :   // run the control loop asynchronously in an OneShotTimer
+    3801             :   // but only if its not already running
+    3802      138590 :   if (!running_async_control_) {
+    3803             : 
+    3804      136502 :     running_async_control_ = true;
+    3805             : 
+    3806      136502 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3807             :   }
+    3808             : }
+    3809             : 
+    3810             : //}
+    3811             : 
+    3812             : /* //{ callbackGNSS() */
+    3813             : 
+    3814       70439 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    3815             : 
+    3816       70439 :   if (!is_initialized_)
+    3817          85 :     return;
+    3818             : 
+    3819      211062 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGNSS");
+    3820      211062 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+    3821             : 
+    3822       70354 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    3823             : }
+    3824             : 
+    3825             : //}
+    3826             : 
+    3827             : /* callbackJoystick() //{ */
+    3828             : 
+    3829           0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+    3830             : 
+    3831           0 :   if (!is_initialized_)
+    3832           0 :     return;
+    3833             : 
+    3834           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackJoystick");
+    3835           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3836             : 
+    3837             :   // copy member variables
+    3838           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    3839           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3840             : 
+    3841           0 :   sensor_msgs::JoyConstPtr joystick_data = msg;
+    3842             : 
+    3843             :   // TODO check if the array is smaller than the largest idx
+    3844           0 :   if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+    3845           0 :     return;
+    3846             :   }
+    3847             : 
+    3848             :   // | ---- switching back to fallback tracker and controller --- |
+    3849             : 
+    3850             :   // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+    3851           0 :   if ((joystick_data->buttons.at(_channel_A_) == 1 || joystick_data->buttons.at(_channel_B_) == 1 || joystick_data->buttons.at(_channel_X_) == 1 ||
+    3852           0 :        joystick_data->buttons.at(_channel_Y_) == 1) &&
+    3853           0 :       active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+    3854             : 
+    3855           0 :     ROS_INFO("[ControlManager]: switching from joystick to normal control");
+    3856             : 
+    3857           0 :     switchTracker(_joystick_fallback_tracker_name_);
+    3858           0 :     switchController(_joystick_fallback_controller_name_);
+    3859             : 
+    3860           0 :     joystick_goto_enabled_ = false;
+    3861             :   }
+    3862             : 
+    3863             :   // | ------- joystick control activation ------- |
+    3864             : 
+    3865             :   // if start button was pressed
+    3866           0 :   if (joystick_data->buttons.at(_channel_start_) == 1) {
+    3867             : 
+    3868           0 :     if (!joystick_start_pressed_) {
+    3869             : 
+    3870           0 :       ROS_INFO("[ControlManager]: joystick start button pressed");
+    3871             : 
+    3872           0 :       joystick_start_pressed_    = true;
+    3873           0 :       joystick_start_press_time_ = ros::Time::now();
+    3874             :     }
+    3875             : 
+    3876           0 :   } else if (joystick_start_pressed_) {
+    3877             : 
+    3878           0 :     ROS_INFO("[ControlManager]: joystick start button released");
+    3879             : 
+    3880           0 :     joystick_start_pressed_    = false;
+    3881           0 :     joystick_start_press_time_ = ros::Time(0);
+    3882             :   }
+    3883             : 
+    3884             :   // | ---------------- Joystick goto activation ---------------- |
+    3885             : 
+    3886             :   // if back button was pressed
+    3887           0 :   if (joystick_data->buttons.at(_channel_back_) == 1) {
+    3888             : 
+    3889           0 :     if (!joystick_back_pressed_) {
+    3890             : 
+    3891           0 :       ROS_INFO("[ControlManager]: joystick back button pressed");
+    3892             : 
+    3893           0 :       joystick_back_pressed_    = true;
+    3894           0 :       joystick_back_press_time_ = ros::Time::now();
+    3895             :     }
+    3896             : 
+    3897           0 :   } else if (joystick_back_pressed_) {
+    3898             : 
+    3899           0 :     ROS_INFO("[ControlManager]: joystick back button released");
+    3900             : 
+    3901           0 :     joystick_back_pressed_    = false;
+    3902           0 :     joystick_back_press_time_ = ros::Time(0);
+    3903             :   }
+    3904             : 
+    3905             :   // | ------------------------ Failsafes ----------------------- |
+    3906             : 
+    3907             :   // if LT and RT buttons are both pressed down
+    3908           0 :   if (joystick_data->axes.at(_channel_LT_) < -0.99 && joystick_data->axes.at(_channel_RT_) < -0.99) {
+    3909             : 
+    3910           0 :     if (!joystick_failsafe_pressed_) {
+    3911             : 
+    3912           0 :       ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+    3913             : 
+    3914           0 :       joystick_failsafe_pressed_    = true;
+    3915           0 :       joystick_failsafe_press_time_ = ros::Time::now();
+    3916             :     }
+    3917             : 
+    3918           0 :   } else if (joystick_failsafe_pressed_) {
+    3919             : 
+    3920           0 :     ROS_INFO("[ControlManager]: joystick Failsafe released");
+    3921             : 
+    3922           0 :     joystick_failsafe_pressed_    = false;
+    3923           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3924             :   }
+    3925             : 
+    3926             :   // if left and right joypads are both pressed down
+    3927           0 :   if (joystick_data->buttons.at(_channel_L_joy_) == 1 && joystick_data->buttons.at(_channel_R_joy_) == 1) {
+    3928             : 
+    3929           0 :     if (!joystick_eland_pressed_) {
+    3930             : 
+    3931           0 :       ROS_INFO("[ControlManager]: joystick eland pressed");
+    3932             : 
+    3933           0 :       joystick_eland_pressed_    = true;
+    3934           0 :       joystick_eland_press_time_ = ros::Time::now();
+    3935             :     }
+    3936             : 
+    3937           0 :   } else if (joystick_eland_pressed_) {
+    3938             : 
+    3939           0 :     ROS_INFO("[ControlManager]: joystick eland released");
+    3940             : 
+    3941           0 :     joystick_eland_pressed_    = false;
+    3942           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3943             :   }
+    3944             : }
+    3945             : 
+    3946             : //}
+    3947             : 
+    3948             : /* //{ callbackHwApiStatus() */
+    3949             : 
+    3950      404356 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+    3951             : 
+    3952      404356 :   if (!is_initialized_)
+    3953         331 :     return;
+    3954             : 
+    3955     1212075 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+    3956     1212075 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+    3957             : 
+    3958      808050 :   mrs_msgs::HwApiStatusConstPtr state = msg;
+    3959             : 
+    3960             :   // | ------ detect and print the changes in offboard mode ----- |
+    3961      404025 :   if (state->offboard) {
+    3962             : 
+    3963      283390 :     if (!offboard_mode_) {
+    3964          88 :       offboard_mode_          = true;
+    3965          88 :       offboard_mode_was_true_ = true;
+    3966          88 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+    3967             :     }
+    3968             : 
+    3969             :   } else {
+    3970             : 
+    3971      120635 :     if (offboard_mode_) {
+    3972           0 :       offboard_mode_ = false;
+    3973           0 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+    3974             :     }
+    3975             :   }
+    3976             : 
+    3977             :   // | --------- detect and print the changes in arming --------- |
+    3978      404025 :   if (state->armed == true) {
+    3979             : 
+    3980      296288 :     if (!armed_) {
+    3981          93 :       armed_ = true;
+    3982          93 :       ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+    3983             :     }
+    3984             : 
+    3985             :   } else {
+    3986             : 
+    3987      107737 :     if (armed_) {
+    3988          20 :       armed_ = false;
+    3989          20 :       ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+    3990             :     }
+    3991             :   }
+    3992             : }
+    3993             : 
+    3994             : //}
+    3995             : 
+    3996             : /* //{ callbackRC() */
+    3997             : 
+    3998       22551 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+    3999             : 
+    4000       22551 :   if (!is_initialized_)
+    4001           0 :     return;
+    4002             : 
+    4003       67653 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackRC");
+    4004       67653 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+    4005             : 
+    4006       45102 :   mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+    4007             : 
+    4008       22551 :   ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+    4009             : 
+    4010             :   // | ------------------- rc joystic control ------------------- |
+    4011             : 
+    4012             :   // when the switch change its position
+    4013       22551 :   if (_rc_goto_enabled_) {
+    4014             : 
+    4015       22551 :     if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+    4016             : 
+    4017           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+    4018             :                          int(rc->channels.size()));
+    4019             : 
+    4020             :     } else {
+    4021             : 
+    4022       22551 :       bool channel_low  = rc->channels.at(_rc_joystick_channel_) < (0.5 - RC_DEADBAND) ? true : false;
+    4023       22551 :       bool channel_high = rc->channels.at(_rc_joystick_channel_) > (0.5 + RC_DEADBAND) ? true : false;
+    4024             : 
+    4025       22551 :       if (channel_low) {
+    4026       20239 :         rc_joystick_channel_was_low_ = true;
+    4027             :       }
+    4028             : 
+    4029             :       // rc control activation
+    4030       22551 :       if (!rc_goto_active_) {
+    4031             : 
+    4032       20239 :         if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+    4033             : 
+    4034           2 :           if (isFlyingNormally()) {
+    4035             : 
+    4036           2 :             ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+    4037             : 
+    4038           2 :             callbacks_enabled_ = false;
+    4039             : 
+    4040           2 :             std_srvs::SetBoolRequest req_goto_out;
+    4041           2 :             req_goto_out.data = false;
+    4042             : 
+    4043           2 :             std_srvs::SetBoolRequest req_enable_callbacks;
+    4044           2 :             req_enable_callbacks.data = callbacks_enabled_;
+    4045             : 
+    4046             :             {
+    4047           4 :               std::scoped_lock lock(mutex_tracker_list_);
+    4048             : 
+    4049             :               // disable callbacks of all trackers
+    4050          14 :               for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4051          12 :                 tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4052             :               }
+    4053             :             }
+    4054             : 
+    4055           2 :             rc_goto_active_ = true;
+    4056             : 
+    4057             :           } else {
+    4058             : 
+    4059           0 :             ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+    4060           2 :           }
+    4061             : 
+    4062       20237 :         } else if (channel_high && !rc_joystick_channel_was_low_) {
+    4063             : 
+    4064           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+    4065             :         }
+    4066             :       }
+    4067             : 
+    4068             :       // rc control deactivation
+    4069       22551 :       if (rc_goto_active_ && channel_low) {
+    4070             : 
+    4071           2 :         ROS_INFO("[ControlManager]: deactivating RC joystick");
+    4072             : 
+    4073           2 :         callbacks_enabled_ = true;
+    4074             : 
+    4075           2 :         std_srvs::SetBoolRequest req_goto_out;
+    4076           2 :         req_goto_out.data = true;
+    4077             : 
+    4078           2 :         std_srvs::SetBoolRequest req_enable_callbacks;
+    4079           2 :         req_enable_callbacks.data = callbacks_enabled_;
+    4080             : 
+    4081             :         {
+    4082           4 :           std::scoped_lock lock(mutex_tracker_list_);
+    4083             : 
+    4084             :           // enable callbacks of all trackers
+    4085          14 :           for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4086          12 :             tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4087             :           }
+    4088             :         }
+    4089             : 
+    4090           2 :         rc_goto_active_ = false;
+    4091             :       }
+    4092             : 
+    4093             :       // do not forget to update the last... variable
+    4094             :       // only do that if its out of the deadband
+    4095       22551 :       if (channel_high || channel_low) {
+    4096       22551 :         rc_joystick_channel_last_value_ = rc->channels.at(_rc_joystick_channel_);
+    4097             :       }
+    4098             :     }
+    4099             :   }
+    4100             : 
+    4101             :   // | ----------------- RC escalating failsafe ----------------- |
+    4102             : 
+    4103       22551 :   if (_rc_escalating_failsafe_enabled_) {
+    4104             : 
+    4105       22551 :     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+    4106             : 
+    4107           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+    4108             :                          int(rc->channels.size()));
+    4109             : 
+    4110             :     } else {
+    4111             : 
+    4112       22551 :       if (rc->channels.at(_rc_escalating_failsafe_channel_) >= _rc_escalating_failsafe_threshold_) {
+    4113             : 
+    4114         142 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+    4115             : 
+    4116         284 :         auto [success, message] = escalatingFailsafe();
+    4117             : 
+    4118         142 :         if (success) {
+    4119           3 :           rc_escalating_failsafe_triggered_ = true;
+    4120             :         }
+    4121             :       }
+    4122             :     }
+    4123             :   }
+    4124             : }
+    4125             : 
+    4126             : //}
+    4127             : 
+    4128             : // | --------------------- topic timeouts --------------------- |
+    4129             : 
+    4130             : /* timeoutUavState() //{ */
+    4131             : 
+    4132           8 : void ControlManager::timeoutUavState(const double& missing_for) {
+    4133             : 
+    4134          16 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    4135             : 
+    4136           8 :   if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+    4137             : 
+    4138             :     // We need to fire up timerFailsafe, which will regularly trigger the controllers
+    4139             :     // in place of the callbackUavState/callbackOdometry().
+    4140             : 
+    4141           1 :     ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+    4142             : 
+    4143           1 :     failsafe();
+    4144             :   }
+    4145           8 : }
+    4146             : 
+    4147             : //}
+    4148             : 
+    4149             : // | -------------------- service callbacks ------------------- |
+    4150             : 
+    4151             : /* //{ callbackSwitchTracker() */
+    4152             : 
+    4153         184 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4154             : 
+    4155         184 :   if (!is_initialized_)
+    4156           0 :     return false;
+    4157             : 
+    4158         184 :   if (failsafe_triggered_ || eland_triggered_) {
+    4159             : 
+    4160           0 :     std::stringstream ss;
+    4161           0 :     ss << "can not switch tracker, eland or failsafe active";
+    4162             : 
+    4163           0 :     res.message = ss.str();
+    4164           0 :     res.success = false;
+    4165             : 
+    4166           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4167             : 
+    4168           0 :     return true;
+    4169             :   }
+    4170             : 
+    4171         184 :   auto [success, response] = switchTracker(req.value);
+    4172             : 
+    4173         184 :   res.success = success;
+    4174         184 :   res.message = response;
+    4175             : 
+    4176         184 :   return true;
+    4177             : }
+    4178             : 
+    4179             : //}
+    4180             : 
+    4181             : /* callbackSwitchController() //{ */
+    4182             : 
+    4183         185 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4184             : 
+    4185         185 :   if (!is_initialized_)
+    4186           0 :     return false;
+    4187             : 
+    4188         185 :   if (failsafe_triggered_ || eland_triggered_) {
+    4189             : 
+    4190           0 :     std::stringstream ss;
+    4191           0 :     ss << "can not switch controller, eland or failsafe active";
+    4192             : 
+    4193           0 :     res.message = ss.str();
+    4194           0 :     res.success = false;
+    4195             : 
+    4196           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4197             : 
+    4198           0 :     return true;
+    4199             :   }
+    4200             : 
+    4201         185 :   auto [success, response] = switchController(req.value);
+    4202             : 
+    4203         185 :   res.success = success;
+    4204         185 :   res.message = response;
+    4205             : 
+    4206         185 :   return true;
+    4207             : }
+    4208             : 
+    4209             : //}
+    4210             : 
+    4211             : /* //{ callbackSwitchTracker() */
+    4212             : 
+    4213           0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4214             : 
+    4215           0 :   if (!is_initialized_)
+    4216           0 :     return false;
+    4217             : 
+    4218           0 :   std::stringstream message;
+    4219             : 
+    4220           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4221             : 
+    4222           0 :     message << "can not reset tracker, eland or failsafe active";
+    4223             : 
+    4224           0 :     res.message = message.str();
+    4225           0 :     res.success = false;
+    4226             : 
+    4227           0 :     ROS_WARN_STREAM("[ControlManager]: " << message.str());
+    4228             : 
+    4229           0 :     return true;
+    4230             :   }
+    4231             : 
+    4232             :   // reactivate the current tracker
+    4233             :   {
+    4234           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4235             : 
+    4236           0 :     std::string tracker_name = _tracker_names_.at(active_tracker_idx_);
+    4237             : 
+    4238           0 :     bool succ = tracker_list_.at(active_tracker_idx_)->resetStatic();
+    4239             : 
+    4240           0 :     if (succ) {
+    4241           0 :       message << "the tracker '" << tracker_name << "' was reset";
+    4242           0 :       ROS_INFO_STREAM("[ControlManager]: " << message.str());
+    4243             :     } else {
+    4244           0 :       message << "the tracker '" << tracker_name << "' reset failed!";
+    4245           0 :       ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+    4246             :     }
+    4247             :   }
+    4248             : 
+    4249           0 :   res.message = message.str();
+    4250           0 :   res.success = true;
+    4251             : 
+    4252           0 :   return true;
+    4253             : }
+    4254             : 
+    4255             : //}
+    4256             : 
+    4257             : /* //{ callbackEHover() */
+    4258             : 
+    4259           2 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4260             : 
+    4261           2 :   if (!is_initialized_)
+    4262           0 :     return false;
+    4263             : 
+    4264           2 :   if (failsafe_triggered_ || eland_triggered_) {
+    4265             : 
+    4266           0 :     std::stringstream ss;
+    4267           0 :     ss << "can not switch controller, eland or failsafe active";
+    4268             : 
+    4269           0 :     res.message = ss.str();
+    4270           0 :     res.success = false;
+    4271             : 
+    4272           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4273             : 
+    4274           0 :     return true;
+    4275             :   }
+    4276             : 
+    4277           2 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+    4278             : 
+    4279           2 :   auto [success, message] = ehover();
+    4280             : 
+    4281           2 :   res.success = success;
+    4282           2 :   res.message = message;
+    4283             : 
+    4284           2 :   return true;
+    4285             : }
+    4286             : 
+    4287             : //}
+    4288             : 
+    4289             : /* callbackFailsafe() //{ */
+    4290             : 
+    4291           4 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4292             : 
+    4293           4 :   if (!is_initialized_)
+    4294           0 :     return false;
+    4295             : 
+    4296           4 :   if (failsafe_triggered_) {
+    4297             : 
+    4298           0 :     std::stringstream ss;
+    4299           0 :     ss << "can not activate failsafe, it is already active";
+    4300             : 
+    4301           0 :     res.message = ss.str();
+    4302           0 :     res.success = false;
+    4303             : 
+    4304           0 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4305             : 
+    4306           0 :     return true;
+    4307             :   }
+    4308             : 
+    4309           4 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+    4310             : 
+    4311           4 :   auto [success, message] = failsafe();
+    4312             : 
+    4313           4 :   res.success = success;
+    4314           4 :   res.message = message;
+    4315             : 
+    4316           4 :   return true;
+    4317             : }
+    4318             : 
+    4319             : //}
+    4320             : 
+    4321             : /* callbackFailsafeEscalating() //{ */
+    4322             : 
+    4323           7 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4324             : 
+    4325           7 :   if (!is_initialized_)
+    4326           0 :     return false;
+    4327             : 
+    4328           7 :   if (_service_escalating_failsafe_enabled_) {
+    4329             : 
+    4330           7 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+    4331             : 
+    4332          14 :     auto [success, message] = escalatingFailsafe();
+    4333             : 
+    4334           7 :     res.success = success;
+    4335           7 :     res.message = message;
+    4336             : 
+    4337             :   } else {
+    4338             : 
+    4339           0 :     std::stringstream ss;
+    4340           0 :     ss << "escalating failsafe is disabled";
+    4341             : 
+    4342           0 :     res.success = false;
+    4343           0 :     res.message = ss.str();
+    4344             : 
+    4345           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+    4346             :   }
+    4347             : 
+    4348           7 :   return true;
+    4349             : }
+    4350             : 
+    4351             : //}
+    4352             : 
+    4353             : /* //{ callbackELand() */
+    4354             : 
+    4355           2 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4356             : 
+    4357           2 :   if (!is_initialized_)
+    4358           0 :     return false;
+    4359             : 
+    4360           2 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+    4361             : 
+    4362           2 :   auto [success, message] = eland();
+    4363             : 
+    4364           2 :   res.success = success;
+    4365           2 :   res.message = message;
+    4366             : 
+    4367           2 :   return true;
+    4368             : }
+    4369             : 
+    4370             : //}
+    4371             : 
+    4372             : /* //{ callbackParachute() */
+    4373             : 
+    4374           0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4375             : 
+    4376           0 :   if (!is_initialized_)
+    4377           0 :     return false;
+    4378             : 
+    4379           0 :   if (!_parachute_enabled_) {
+    4380             : 
+    4381           0 :     std::stringstream ss;
+    4382           0 :     ss << "parachute disabled";
+    4383           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4384           0 :     res.message = ss.str();
+    4385           0 :     res.success = false;
+    4386             :   }
+    4387             : 
+    4388           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+    4389             : 
+    4390           0 :   auto [success, message] = deployParachute();
+    4391             : 
+    4392           0 :   res.success = success;
+    4393           0 :   res.message = message;
+    4394             : 
+    4395           0 :   return true;
+    4396             : }
+    4397             : 
+    4398             : //}
+    4399             : 
+    4400             : /* //{ callbackSetMinZ() */
+    4401             : 
+    4402           0 : bool ControlManager::callbackSetMinZ([[maybe_unused]] mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res) {
+    4403             : 
+    4404           0 :   if (!is_initialized_)
+    4405           0 :     return false;
+    4406             : 
+    4407           0 :   if (!use_safety_area_) {
+    4408           0 :     res.success = true;
+    4409           0 :     res.message = "safety area is disabled";
+    4410           0 :     return true;
+    4411             :   }
+    4412             : 
+    4413             :   // | -------- transform min_z to the safety area frame -------- |
+    4414             : 
+    4415           0 :   mrs_msgs::ReferenceStamped point;
+    4416           0 :   point.header               = req.header;
+    4417           0 :   point.reference.position.z = req.value;
+    4418             : 
+    4419           0 :   auto result = transformer_->transformSingle(point, _safety_area_vertical_frame_);
+    4420             : 
+    4421           0 :   if (result) {
+    4422             : 
+    4423           0 :     _safety_area_min_z_ = result.value().reference.position.z;
+    4424             : 
+    4425           0 :     res.success = true;
+    4426           0 :     res.message = "safety area's min z updated";
+    4427             : 
+    4428             :   } else {
+    4429             : 
+    4430           0 :     res.success = false;
+    4431           0 :     res.message = "could not transform the value to safety area's vertical frame";
+    4432             :   }
+    4433             : 
+    4434           0 :   return true;
+    4435             : }
+    4436             : 
+    4437             : //}
+    4438             : 
+    4439             : /* //{ callbackToggleOutput() */
+    4440             : 
+    4441          95 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4442             : 
+    4443          95 :   if (!is_initialized_)
+    4444           0 :     return false;
+    4445             : 
+    4446          95 :   ROS_INFO("[ControlManager]: toggling output by service");
+    4447             : 
+    4448             :   // copy member variables
+    4449         190 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4450             : 
+    4451         190 :   std::stringstream ss;
+    4452             : 
+    4453          95 :   bool prereq_check = true;
+    4454             : 
+    4455             :   {
+    4456         190 :     mrs_msgs::ReferenceStamped current_coord;
+    4457          95 :     current_coord.header.frame_id      = uav_state.header.frame_id;
+    4458          95 :     current_coord.reference.position.x = uav_state.pose.position.x;
+    4459          95 :     current_coord.reference.position.y = uav_state.pose.position.y;
+    4460             : 
+    4461          95 :     if (!isPointInSafetyArea2d(current_coord)) {
+    4462           1 :       ss << "cannot toggle output, the UAV is outside of the safety area!";
+    4463           1 :       prereq_check = false;
+    4464             :     }
+    4465             :   }
+    4466             : 
+    4467          95 :   if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+    4468           0 :     ss << "cannot toggle output ON, we landed in emergency";
+    4469           0 :     prereq_check = false;
+    4470             :   }
+    4471             : 
+    4472          95 :   if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+    4473           0 :     ss << "cannot toggle output ON, missing HW API status!";
+    4474           0 :     prereq_check = false;
+    4475             :   }
+    4476             : 
+    4477          95 :   if (!prereq_check) {
+    4478             : 
+    4479           1 :     res.message = ss.str();
+    4480           1 :     res.success = false;
+    4481             : 
+    4482           1 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4483             : 
+    4484           1 :     return false;
+    4485             : 
+    4486             :   } else {
+    4487             : 
+    4488          94 :     toggleOutput(req.data);
+    4489             : 
+    4490          94 :     ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+    4491          94 :     res.message = ss.str();
+    4492          94 :     res.success = true;
+    4493             : 
+    4494          94 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4495             : 
+    4496          94 :     publishDiagnostics();
+    4497             : 
+    4498          94 :     return true;
+    4499             :   }
+    4500             : }
+    4501             : 
+    4502             : //}
+    4503             : 
+    4504             : /* callbackArm() //{ */
+    4505             : 
+    4506           7 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4507             : 
+    4508           7 :   if (!is_initialized_)
+    4509           0 :     return false;
+    4510             : 
+    4511           7 :   ROS_INFO("[ControlManager]: arming by service");
+    4512             : 
+    4513          14 :   std::stringstream ss;
+    4514             : 
+    4515           7 :   if (failsafe_triggered_ || eland_triggered_) {
+    4516             : 
+    4517           0 :     ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+    4518             : 
+    4519           0 :     res.message = ss.str();
+    4520           0 :     res.success = false;
+    4521             : 
+    4522           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4523             : 
+    4524           0 :     return true;
+    4525             :   }
+    4526             : 
+    4527           7 :   if (req.data) {
+    4528             : 
+    4529           0 :     ss << "this service is not allowed to arm the UAV";
+    4530           0 :     res.success = false;
+    4531           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4532             : 
+    4533             :   } else {
+    4534             : 
+    4535          14 :     auto [success, message] = arming(false);
+    4536             : 
+    4537           7 :     if (success) {
+    4538             : 
+    4539           7 :       ss << "disarmed";
+    4540           7 :       res.success = true;
+    4541           7 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4542             : 
+    4543             :     } else {
+    4544             : 
+    4545           0 :       ss << "could not disarm: " << message;
+    4546           0 :       res.success = false;
+    4547           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4548             :     }
+    4549             :   }
+    4550             : 
+    4551           7 :   res.message = ss.str();
+    4552             : 
+    4553           7 :   return true;
+    4554             : }
+    4555             : 
+    4556             : //}
+    4557             : 
+    4558             : /* //{ callbackEnableCallbacks() */
+    4559             : 
+    4560          94 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4561             : 
+    4562          94 :   if (!is_initialized_)
+    4563           0 :     return false;
+    4564             : 
+    4565          94 :   setCallbacks(req.data);
+    4566             : 
+    4567          94 :   std::stringstream ss;
+    4568             : 
+    4569          94 :   ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+    4570             : 
+    4571          94 :   res.message = ss.str();
+    4572          94 :   res.success = true;
+    4573             : 
+    4574          94 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4575             : 
+    4576          94 :   return true;
+    4577             : }
+    4578             : 
+    4579             : //}
+    4580             : 
+    4581             : /* callbackSetConstraints() //{ */
+    4582             : 
+    4583          97 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+    4584             : 
+    4585          97 :   if (!is_initialized_) {
+    4586           0 :     res.message = "not initialized";
+    4587           0 :     res.success = false;
+    4588           0 :     return true;
+    4589             :   }
+    4590             : 
+    4591             :   {
+    4592         194 :     std::scoped_lock lock(mutex_constraints_);
+    4593             : 
+    4594          97 :     current_constraints_ = req;
+    4595             : 
+    4596          97 :     auto enforced = enforceControllersConstraints(current_constraints_);
+    4597             : 
+    4598          97 :     if (enforced) {
+    4599           0 :       sanitized_constraints_      = enforced.value();
+    4600           0 :       constraints_being_enforced_ = true;
+    4601             :     } else {
+    4602          97 :       sanitized_constraints_      = req;
+    4603          97 :       constraints_being_enforced_ = false;
+    4604             :     }
+    4605             : 
+    4606          97 :     got_constraints_ = true;
+    4607             : 
+    4608          97 :     setConstraintsToControllers(current_constraints_);
+    4609          97 :     setConstraintsToTrackers(sanitized_constraints_);
+    4610             :   }
+    4611             : 
+    4612          97 :   res.message = "setting constraints";
+    4613          97 :   res.success = true;
+    4614             : 
+    4615          97 :   return true;
+    4616             : }
+    4617             : 
+    4618             : //}
+    4619             : 
+    4620             : /* //{ callbackEmergencyReference() */
+    4621             : 
+    4622          94 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    4623             : 
+    4624          94 :   if (!is_initialized_)
+    4625           0 :     return false;
+    4626             : 
+    4627         188 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4628             : 
+    4629          94 :   callbacks_enabled_ = false;
+    4630             : 
+    4631          94 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    4632             : 
+    4633         188 :   std::stringstream ss;
+    4634             : 
+    4635             :   // transform the reference to the current frame
+    4636         188 :   mrs_msgs::ReferenceStamped original_reference;
+    4637          94 :   original_reference.header    = req.header;
+    4638          94 :   original_reference.reference = req.reference;
+    4639             : 
+    4640         188 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    4641             : 
+    4642          94 :   if (!ret) {
+    4643             : 
+    4644           0 :     ss << "the emergency reference could not be transformed";
+    4645             : 
+    4646           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4647           0 :     res.message = ss.str();
+    4648           0 :     res.success = false;
+    4649           0 :     return true;
+    4650             :   }
+    4651             : 
+    4652          94 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    4653             : 
+    4654          94 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    4655             : 
+    4656          94 :   mrs_msgs::ReferenceSrvRequest req_goto_out;
+    4657          94 :   req_goto_out.reference = transformed_reference.reference;
+    4658             : 
+    4659             :   {
+    4660         188 :     std::scoped_lock lock(mutex_tracker_list_);
+    4661             : 
+    4662             :     // disable callbacks of all trackers
+    4663          94 :     req_enable_callbacks.data = false;
+    4664         658 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4665         564 :       tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4666             :     }
+    4667             : 
+    4668             :     // enable the callbacks for the active tracker
+    4669          94 :     req_enable_callbacks.data = true;
+    4670          94 :     tracker_list_.at(active_tracker_idx_)
+    4671          94 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4672             : 
+    4673             :     // call the setReference()
+    4674          94 :     tracker_response = tracker_list_.at(active_tracker_idx_)
+    4675          94 :                            ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    4676             : 
+    4677             :     // disable the callbacks back again
+    4678          94 :     req_enable_callbacks.data = false;
+    4679          94 :     tracker_list_.at(active_tracker_idx_)
+    4680          94 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4681             : 
+    4682          94 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    4683          94 :       res.message = tracker_response->message;
+    4684          94 :       res.success = tracker_response->success;
+    4685             :     } else {
+    4686           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+    4687           0 :       res.message = ss.str();
+    4688           0 :       res.success = false;
+    4689             :     }
+    4690             :   }
+    4691             : 
+    4692          94 :   return true;
+    4693             : }
+    4694             : 
+    4695             : //}
+    4696             : 
+    4697             : /* callbackPirouette() //{ */
+    4698             : 
+    4699           0 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4700             : 
+    4701           0 :   if (!is_initialized_)
+    4702           0 :     return false;
+    4703             : 
+    4704             :   // copy member variables
+    4705           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4706             : 
+    4707             :   double uav_heading;
+    4708             :   try {
+    4709           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    4710             :   }
+    4711           0 :   catch (...) {
+    4712           0 :     std::stringstream ss;
+    4713           0 :     ss << "could not calculate the UAV heading to initialize the pirouette";
+    4714             : 
+    4715           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4716             : 
+    4717           0 :     res.message = ss.str();
+    4718           0 :     res.success = false;
+    4719             : 
+    4720           0 :     return false;
+    4721             :   }
+    4722             : 
+    4723           0 :   if (_pirouette_enabled_) {
+    4724           0 :     res.success = false;
+    4725           0 :     res.message = "already active";
+    4726           0 :     return true;
+    4727             :   }
+    4728             : 
+    4729           0 :   if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+    4730             : 
+    4731           0 :     std::stringstream ss;
+    4732           0 :     ss << "can not activate the pirouette, eland or failsafe active";
+    4733             : 
+    4734           0 :     res.message = ss.str();
+    4735           0 :     res.success = false;
+    4736             : 
+    4737           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4738             : 
+    4739           0 :     return true;
+    4740             :   }
+    4741             : 
+    4742           0 :   _pirouette_enabled_ = true;
+    4743             : 
+    4744           0 :   setCallbacks(false);
+    4745             : 
+    4746           0 :   pirouette_initial_heading_ = uav_heading;
+    4747           0 :   pirouette_iterator_        = 0;
+    4748           0 :   timer_pirouette_.start();
+    4749             : 
+    4750           0 :   res.success = true;
+    4751           0 :   res.message = "activated";
+    4752             : 
+    4753           0 :   return true;
+    4754             : }
+    4755             : 
+    4756             : //}
+    4757             : 
+    4758             : /* callbackUseJoystick() //{ */
+    4759             : 
+    4760           0 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4761             : 
+    4762           0 :   if (!is_initialized_) {
+    4763           0 :     return false;
+    4764             :   }
+    4765             : 
+    4766           0 :   std::stringstream ss;
+    4767             : 
+    4768             :   {
+    4769           0 :     auto [success, response] = switchTracker(_joystick_tracker_name_);
+    4770             : 
+    4771           0 :     if (!success) {
+    4772             : 
+    4773           0 :       ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+    4774           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4775             : 
+    4776           0 :       res.success = false;
+    4777           0 :       res.message = ss.str();
+    4778             : 
+    4779           0 :       return true;
+    4780             :     }
+    4781             :   }
+    4782             : 
+    4783           0 :   auto [success, response] = switchController(_joystick_controller_name_);
+    4784             : 
+    4785           0 :   if (!success) {
+    4786             : 
+    4787           0 :     ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+    4788           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4789             : 
+    4790           0 :     res.success = false;
+    4791           0 :     res.message = ss.str();
+    4792             : 
+    4793             :     // switch back to hover tracker
+    4794           0 :     switchTracker(_ehover_tracker_name_);
+    4795             : 
+    4796             :     // switch back to safety controller
+    4797           0 :     switchController(_eland_controller_name_);
+    4798             : 
+    4799           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4800             : 
+    4801           0 :     return true;
+    4802             :   }
+    4803             : 
+    4804           0 :   ss << "switched to joystick control";
+    4805             : 
+    4806           0 :   res.success = true;
+    4807           0 :   res.message = ss.str();
+    4808             : 
+    4809           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4810             : 
+    4811           0 :   return true;
+    4812             : }
+    4813             : 
+    4814             : //}
+    4815             : 
+    4816             : /* //{ callbackHover() */
+    4817             : 
+    4818           0 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4819             : 
+    4820           0 :   if (!is_initialized_)
+    4821           0 :     return false;
+    4822             : 
+    4823           0 :   auto [success, message] = hover();
+    4824             : 
+    4825           0 :   res.success = success;
+    4826           0 :   res.message = message;
+    4827             : 
+    4828           0 :   return true;
+    4829             : }
+    4830             : 
+    4831             : //}
+    4832             : 
+    4833             : /* //{ callbackStartTrajectoryTracking() */
+    4834             : 
+    4835           2 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4836             : 
+    4837           2 :   if (!is_initialized_)
+    4838           0 :     return false;
+    4839             : 
+    4840           2 :   auto [success, message] = startTrajectoryTracking();
+    4841             : 
+    4842           2 :   res.success = success;
+    4843           2 :   res.message = message;
+    4844             : 
+    4845           2 :   return true;
+    4846             : }
+    4847             : 
+    4848             : //}
+    4849             : 
+    4850             : /* //{ callbackStopTrajectoryTracking() */
+    4851             : 
+    4852           1 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4853             : 
+    4854           1 :   if (!is_initialized_)
+    4855           0 :     return false;
+    4856             : 
+    4857           1 :   auto [success, message] = stopTrajectoryTracking();
+    4858             : 
+    4859           1 :   res.success = success;
+    4860           1 :   res.message = message;
+    4861             : 
+    4862           1 :   return true;
+    4863             : }
+    4864             : 
+    4865             : //}
+    4866             : 
+    4867             : /* //{ callbackResumeTrajectoryTracking() */
+    4868             : 
+    4869           1 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4870             : 
+    4871           1 :   if (!is_initialized_)
+    4872           0 :     return false;
+    4873             : 
+    4874           1 :   auto [success, message] = resumeTrajectoryTracking();
+    4875             : 
+    4876           1 :   res.success = success;
+    4877           1 :   res.message = message;
+    4878             : 
+    4879           1 :   return true;
+    4880             : }
+    4881             : 
+    4882             : //}
+    4883             : 
+    4884             : /* //{ callbackGotoTrajectoryStart() */
+    4885             : 
+    4886           2 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4887             : 
+    4888           2 :   if (!is_initialized_)
+    4889           0 :     return false;
+    4890             : 
+    4891           2 :   auto [success, message] = gotoTrajectoryStart();
+    4892             : 
+    4893           2 :   res.success = success;
+    4894           2 :   res.message = message;
+    4895             : 
+    4896           2 :   return true;
+    4897             : }
+    4898             : 
+    4899             : //}
+    4900             : 
+    4901             : /* //{ callbackTransformReference() */
+    4902             : 
+    4903           0 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+    4904             : 
+    4905           0 :   if (!is_initialized_)
+    4906           0 :     return false;
+    4907             : 
+    4908             :   // transform the reference to the current frame
+    4909           0 :   mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+    4910             : 
+    4911           0 :   if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+    4912             : 
+    4913           0 :     res.reference = ret.value();
+    4914           0 :     res.message   = "transformation successful";
+    4915           0 :     res.success   = true;
+    4916           0 :     return true;
+    4917             : 
+    4918             :   } else {
+    4919             : 
+    4920           0 :     res.message = "the reference could not be transformed";
+    4921           0 :     res.success = false;
+    4922           0 :     return true;
+    4923             :   }
+    4924             : 
+    4925             :   return true;
+    4926             : }
+    4927             : 
+    4928             : //}
+    4929             : 
+    4930             : /* //{ callbackTransformPose() */
+    4931             : 
+    4932           0 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+    4933             : 
+    4934           0 :   if (!is_initialized_)
+    4935           0 :     return false;
+    4936             : 
+    4937             :   // transform the reference to the current frame
+    4938           0 :   geometry_msgs::PoseStamped transformed_pose = req.pose;
+    4939             : 
+    4940           0 :   if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+    4941             : 
+    4942           0 :     res.pose    = ret.value();
+    4943           0 :     res.message = "transformation successful";
+    4944           0 :     res.success = true;
+    4945           0 :     return true;
+    4946             : 
+    4947             :   } else {
+    4948             : 
+    4949           0 :     res.message = "the pose could not be transformed";
+    4950           0 :     res.success = false;
+    4951           0 :     return true;
+    4952             :   }
+    4953             : 
+    4954             :   return true;
+    4955             : }
+    4956             : 
+    4957             : //}
+    4958             : 
+    4959             : /* //{ callbackTransformVector3() */
+    4960             : 
+    4961           0 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+    4962             : 
+    4963           0 :   if (!is_initialized_)
+    4964           0 :     return false;
+    4965             : 
+    4966             :   // transform the reference to the current frame
+    4967           0 :   geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+    4968             : 
+    4969           0 :   if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+    4970             : 
+    4971           0 :     res.vector  = ret.value();
+    4972           0 :     res.message = "transformation successful";
+    4973           0 :     res.success = true;
+    4974           0 :     return true;
+    4975             : 
+    4976             :   } else {
+    4977             : 
+    4978           0 :     res.message = "the twist could not be transformed";
+    4979           0 :     res.success = false;
+    4980           0 :     return true;
+    4981             :   }
+    4982             : 
+    4983             :   return true;
+    4984             : }
+    4985             : 
+    4986             : //}
+    4987             : 
+    4988             : /* //{ callbackEnableBumper() */
+    4989             : 
+    4990           0 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4991             : 
+    4992           0 :   if (!is_initialized_)
+    4993           0 :     return false;
+    4994             : 
+    4995           0 :   bumper_enabled_ = req.data;
+    4996             : 
+    4997           0 :   std::stringstream ss;
+    4998             : 
+    4999           0 :   ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+    5000             : 
+    5001           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    5002             : 
+    5003           0 :   res.success = true;
+    5004           0 :   res.message = ss.str();
+    5005             : 
+    5006           0 :   return true;
+    5007             : }
+    5008             : 
+    5009             : //}
+    5010             : 
+    5011             : /* //{ callbackUseSafetyArea() */
+    5012             : 
+    5013           0 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    5014             : 
+    5015           0 :   if (!is_initialized_)
+    5016           0 :     return false;
+    5017             : 
+    5018           0 :   use_safety_area_ = req.data;
+    5019             : 
+    5020           0 :   std::stringstream ss;
+    5021             : 
+    5022           0 :   ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+    5023             : 
+    5024           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    5025             : 
+    5026           0 :   res.success = true;
+    5027           0 :   res.message = ss.str();
+    5028             : 
+    5029           0 :   return true;
+    5030             : }
+    5031             : 
+    5032             : //}
+    5033             : 
+    5034             : /* //{ callbackGetMinZ() */
+    5035             : 
+    5036           0 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+    5037             : 
+    5038           0 :   if (!is_initialized_) {
+    5039           0 :     return false;
+    5040             :   }
+    5041             : 
+    5042           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5043             : 
+    5044           0 :   res.success = true;
+    5045           0 :   res.value   = getMinZ(uav_state.header.frame_id);
+    5046             : 
+    5047           0 :   return true;
+    5048             : }
+    5049             : 
+    5050             : //}
+    5051             : 
+    5052             : /* //{ callbackValidateReference() */
+    5053             : 
+    5054           0 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5055             : 
+    5056           0 :   if (!is_initialized_) {
+    5057           0 :     res.message = "not initialized";
+    5058           0 :     res.success = false;
+    5059           0 :     return true;
+    5060             :   }
+    5061             : 
+    5062           0 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5063           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5064           0 :     res.message = "NaNs/infs in input!";
+    5065           0 :     res.success = false;
+    5066           0 :     return true;
+    5067             :   }
+    5068             : 
+    5069             :   // copy member variables
+    5070           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5071           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5072             : 
+    5073             :   // transform the reference to the current frame
+    5074           0 :   mrs_msgs::ReferenceStamped original_reference;
+    5075           0 :   original_reference.header    = req.reference.header;
+    5076           0 :   original_reference.reference = req.reference.reference;
+    5077             : 
+    5078           0 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5079             : 
+    5080           0 :   if (!ret) {
+    5081             : 
+    5082           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5083           0 :     res.message = "the reference could not be transformed";
+    5084           0 :     res.success = false;
+    5085           0 :     return true;
+    5086             :   }
+    5087             : 
+    5088           0 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5089             : 
+    5090           0 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5091           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5092           0 :     res.message = "the point is outside of the safety area";
+    5093           0 :     res.success = false;
+    5094           0 :     return true;
+    5095             :   }
+    5096             : 
+    5097           0 :   if (last_tracker_cmd) {
+    5098             : 
+    5099           0 :     mrs_msgs::ReferenceStamped from_point;
+    5100           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5101           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5102           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5103           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5104             : 
+    5105           0 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5106           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5107           0 :       res.message = "the path is going outside the safety area";
+    5108           0 :       res.success = false;
+    5109           0 :       return true;
+    5110             :     }
+    5111             :   }
+    5112             : 
+    5113           0 :   res.message = "the reference is ok";
+    5114           0 :   res.success = true;
+    5115           0 :   return true;
+    5116             : }
+    5117             : 
+    5118             : //}
+    5119             : 
+    5120             : /* //{ callbackValidateReference2d() */
+    5121             : 
+    5122        9789 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5123             : 
+    5124        9789 :   if (!is_initialized_) {
+    5125           0 :     res.message = "not initialized";
+    5126           0 :     res.success = false;
+    5127           0 :     return true;
+    5128             :   }
+    5129             : 
+    5130        9789 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5131           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5132           0 :     res.message = "NaNs/infs in input!";
+    5133           0 :     res.success = false;
+    5134           0 :     return true;
+    5135             :   }
+    5136             : 
+    5137             :   // copy member variables
+    5138       19578 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5139       19578 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5140             : 
+    5141             :   // transform the reference to the current frame
+    5142       19578 :   mrs_msgs::ReferenceStamped original_reference;
+    5143        9789 :   original_reference.header    = req.reference.header;
+    5144        9789 :   original_reference.reference = req.reference.reference;
+    5145             : 
+    5146       19578 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5147             : 
+    5148        9789 :   if (!ret) {
+    5149             : 
+    5150           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5151           0 :     res.message = "the reference could not be transformed";
+    5152           0 :     res.success = false;
+    5153           0 :     return true;
+    5154             :   }
+    5155             : 
+    5156       19578 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5157             : 
+    5158        9789 :   if (!isPointInSafetyArea2d(transformed_reference)) {
+    5159          77 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5160          77 :     res.message = "the point is outside of the safety area";
+    5161          77 :     res.success = false;
+    5162          77 :     return true;
+    5163             :   }
+    5164             : 
+    5165        9712 :   if (last_tracker_cmd) {
+    5166             : 
+    5167          31 :     mrs_msgs::ReferenceStamped from_point;
+    5168          31 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5169          31 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5170          31 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5171          31 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5172             : 
+    5173          31 :     if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+    5174           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5175           0 :       res.message = "the path is going outside the safety area";
+    5176           0 :       res.success = false;
+    5177           0 :       return true;
+    5178             :     }
+    5179             :   }
+    5180             : 
+    5181        9712 :   res.message = "the reference is ok";
+    5182        9712 :   res.success = true;
+    5183        9712 :   return true;
+    5184             : }
+    5185             : 
+    5186             : //}
+    5187             : 
+    5188             : /* //{ callbackValidateReferenceList() */
+    5189             : 
+    5190           0 : bool ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res) {
+    5191             : 
+    5192           0 :   if (!is_initialized_) {
+    5193           0 :     res.message = "not initialized";
+    5194           0 :     return false;
+    5195             :   }
+    5196             : 
+    5197             :   // copy member variables
+    5198           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5199           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5200             : 
+    5201             :   // get the transformer
+    5202           0 :   auto ret = transformer_->getTransform(uav_state.header.frame_id, req.list.header.frame_id, req.list.header.stamp);
+    5203             : 
+    5204           0 :   if (!ret) {
+    5205             : 
+    5206           0 :     ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+    5207           0 :     res.message = "could not find transform";
+    5208           0 :     return false;
+    5209             :   }
+    5210             : 
+    5211           0 :   geometry_msgs::TransformStamped tf = ret.value();
+    5212             : 
+    5213           0 :   for (int i = 0; i < int(req.list.list.size()); i++) {
+    5214             : 
+    5215           0 :     res.success.push_back(true);
+    5216             : 
+    5217           0 :     mrs_msgs::ReferenceStamped original_reference;
+    5218           0 :     original_reference.header    = req.list.header;
+    5219           0 :     original_reference.reference = req.list.list.at(i);
+    5220             : 
+    5221           0 :     res.success.at(i) = validateReference(original_reference.reference, "ControlManager", "reference_list");
+    5222             : 
+    5223           0 :     auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5224             : 
+    5225           0 :     if (!ret) {
+    5226             : 
+    5227           0 :       ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+    5228           0 :       res.success.at(i) = false;
+    5229             :     }
+    5230             : 
+    5231           0 :     mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5232             : 
+    5233           0 :     if (!isPointInSafetyArea3d(transformed_reference)) {
+    5234           0 :       res.success.at(i) = false;
+    5235             :     }
+    5236             : 
+    5237           0 :     if (last_tracker_cmd) {
+    5238             : 
+    5239           0 :       mrs_msgs::ReferenceStamped from_point;
+    5240           0 :       from_point.header.frame_id      = uav_state.header.frame_id;
+    5241           0 :       from_point.reference.position.x = last_tracker_cmd->position.x;
+    5242           0 :       from_point.reference.position.y = last_tracker_cmd->position.y;
+    5243           0 :       from_point.reference.position.z = last_tracker_cmd->position.z;
+    5244             : 
+    5245           0 :       if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5246           0 :         res.success.at(i) = false;
+    5247             :       }
+    5248             :     }
+    5249             :   }
+    5250             : 
+    5251           0 :   res.message = "references were checked";
+    5252           0 :   return true;
+    5253             : }
+    5254             : 
+    5255             : //}
+    5256             : 
+    5257             : // | -------------- setpoint topics and services -------------- |
+    5258             : 
+    5259             : /* //{ callbackReferenceService() */
+    5260             : 
+    5261           0 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    5262             : 
+    5263           0 :   if (!is_initialized_) {
+    5264           0 :     res.message = "not initialized";
+    5265           0 :     res.success = false;
+    5266           0 :     return true;
+    5267             :   }
+    5268             : 
+    5269           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceService");
+    5270           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5271             : 
+    5272           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5273           0 :   des_reference.header    = req.header;
+    5274           0 :   des_reference.reference = req.reference;
+    5275             : 
+    5276           0 :   auto [success, message] = setReference(des_reference);
+    5277             : 
+    5278           0 :   res.success = success;
+    5279           0 :   res.message = message;
+    5280             : 
+    5281           0 :   return true;
+    5282             : }
+    5283             : 
+    5284             : //}
+    5285             : 
+    5286             : /* //{ callbackReferenceTopic() */
+    5287             : 
+    5288           0 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+    5289             : 
+    5290           0 :   if (!is_initialized_)
+    5291           0 :     return;
+    5292             : 
+    5293           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+    5294           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5295             : 
+    5296           0 :   setReference(*msg);
+    5297             : }
+    5298             : 
+    5299             : //}
+    5300             : 
+    5301             : /* //{ callbackVelocityReferenceService() */
+    5302             : 
+    5303         631 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request&  req,
+    5304             :                                                       mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+    5305             : 
+    5306         631 :   if (!is_initialized_) {
+    5307           0 :     res.message = "not initialized";
+    5308           0 :     res.success = false;
+    5309           0 :     return true;
+    5310             :   }
+    5311             : 
+    5312        1893 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+    5313        1893 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5314             : 
+    5315        1262 :   mrs_msgs::VelocityReferenceStamped des_reference;
+    5316         631 :   des_reference = req.reference;
+    5317             : 
+    5318         631 :   auto [success, message] = setVelocityReference(des_reference);
+    5319             : 
+    5320         631 :   res.success = success;
+    5321         631 :   res.message = message;
+    5322             : 
+    5323         631 :   return true;
+    5324             : }
+    5325             : 
+    5326             : //}
+    5327             : 
+    5328             : /* //{ callbackVelocityReferenceTopic() */
+    5329             : 
+    5330          89 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+    5331             : 
+    5332          89 :   if (!is_initialized_)
+    5333           0 :     return;
+    5334             : 
+    5335         267 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+    5336         178 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5337             : 
+    5338          89 :   setVelocityReference(*msg);
+    5339             : }
+    5340             : 
+    5341             : //}
+    5342             : 
+    5343             : /* //{ callbackTrajectoryReferenceService() */
+    5344             : 
+    5345           4 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+    5346             : 
+    5347           4 :   if (!is_initialized_) {
+    5348           0 :     res.message = "not initialized";
+    5349           0 :     res.success = false;
+    5350           0 :     return true;
+    5351             :   }
+    5352             : 
+    5353          12 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+    5354          12 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5355             : 
+    5356           8 :   auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+    5357             : 
+    5358           4 :   res.success          = success;
+    5359           4 :   res.message          = message;
+    5360           4 :   res.modified         = modified;
+    5361           4 :   res.tracker_names    = tracker_names;
+    5362           4 :   res.tracker_messages = tracker_messages;
+    5363             : 
+    5364          28 :   for (size_t i = 0; i < tracker_successes.size(); i++) {
+    5365          24 :     res.tracker_successes.push_back(tracker_successes.at(i));
+    5366             :   }
+    5367             : 
+    5368           4 :   return true;
+    5369             : }
+    5370             : 
+    5371             : //}
+    5372             : 
+    5373             : /* //{ callbackTrajectoryReferenceTopic() */
+    5374             : 
+    5375           2 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+    5376             : 
+    5377           2 :   if (!is_initialized_)
+    5378           0 :     return;
+    5379             : 
+    5380           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+    5381           4 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5382             : 
+    5383           2 :   setTrajectoryReference(*msg);
+    5384             : }
+    5385             : 
+    5386             : //}
+    5387             : 
+    5388             : // | ------------- human-callable "goto" services ------------- |
+    5389             : 
+    5390             : /* //{ callbackGoto() */
+    5391             : 
+    5392          27 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5393             : 
+    5394          27 :   if (!is_initialized_) {
+    5395           0 :     res.message = "not initialized";
+    5396           0 :     res.success = false;
+    5397           0 :     return true;
+    5398             :   }
+    5399             : 
+    5400          81 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGoto");
+    5401          81 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+    5402             : 
+    5403          54 :   mrs_msgs::ReferenceStamped des_reference;
+    5404          27 :   des_reference.header.frame_id      = "";
+    5405          27 :   des_reference.header.stamp         = ros::Time(0);
+    5406          27 :   des_reference.reference.position.x = req.goal.at(REF_X);
+    5407          27 :   des_reference.reference.position.y = req.goal.at(REF_Y);
+    5408          27 :   des_reference.reference.position.z = req.goal.at(REF_Z);
+    5409          27 :   des_reference.reference.heading    = req.goal.at(REF_HEADING);
+    5410             : 
+    5411          54 :   auto [success, message] = setReference(des_reference);
+    5412             : 
+    5413          27 :   res.success = success;
+    5414          27 :   res.message = message;
+    5415             : 
+    5416          27 :   return true;
+    5417             : }
+    5418             : 
+    5419             : //}
+    5420             : 
+    5421             : /* //{ callbackGotoFcu() */
+    5422             : 
+    5423           0 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5424             : 
+    5425           0 :   if (!is_initialized_) {
+    5426           0 :     res.message = "not initialized";
+    5427           0 :     res.success = false;
+    5428           0 :     return true;
+    5429             :   }
+    5430             : 
+    5431           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+    5432           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+    5433             : 
+    5434           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5435           0 :   des_reference.header.frame_id      = "fcu_untilted";
+    5436           0 :   des_reference.header.stamp         = ros::Time(0);
+    5437           0 :   des_reference.reference.position.x = req.goal.at(REF_X);
+    5438           0 :   des_reference.reference.position.y = req.goal.at(REF_Y);
+    5439           0 :   des_reference.reference.position.z = req.goal.at(REF_Z);
+    5440           0 :   des_reference.reference.heading    = req.goal.at(REF_HEADING);
+    5441             : 
+    5442           0 :   auto [success, message] = setReference(des_reference);
+    5443             : 
+    5444           0 :   res.success = success;
+    5445           0 :   res.message = message;
+    5446             : 
+    5447           0 :   return true;
+    5448             : }
+    5449             : 
+    5450             : //}
+    5451             : 
+    5452             : /* //{ callbackGotoRelative() */
+    5453             : 
+    5454          23 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5455             : 
+    5456          23 :   if (!is_initialized_) {
+    5457           0 :     res.message = "not initialized";
+    5458           0 :     res.success = false;
+    5459           0 :     return true;
+    5460             :   }
+    5461             : 
+    5462          69 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+    5463          69 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+    5464             : 
+    5465          46 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5466             : 
+    5467          23 :   if (!last_tracker_cmd) {
+    5468           0 :     res.message = "not flying";
+    5469           0 :     res.success = false;
+    5470           0 :     return true;
+    5471             :   }
+    5472             : 
+    5473          46 :   mrs_msgs::ReferenceStamped des_reference;
+    5474          23 :   des_reference.header.frame_id      = "";
+    5475          23 :   des_reference.header.stamp         = ros::Time(0);
+    5476          23 :   des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal.at(REF_X);
+    5477          23 :   des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal.at(REF_Y);
+    5478          23 :   des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal.at(REF_Z);
+    5479          23 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal.at(REF_HEADING);
+    5480             : 
+    5481          46 :   auto [success, message] = setReference(des_reference);
+    5482             : 
+    5483          23 :   res.success = success;
+    5484          23 :   res.message = message;
+    5485             : 
+    5486          23 :   return true;
+    5487             : }
+    5488             : 
+    5489             : //}
+    5490             : 
+    5491             : /* //{ callbackGotoAltitude() */
+    5492             : 
+    5493           0 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5494             : 
+    5495           0 :   if (!is_initialized_) {
+    5496           0 :     res.message = "not initialized";
+    5497           0 :     res.success = false;
+    5498           0 :     return true;
+    5499             :   }
+    5500             : 
+    5501           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+    5502           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+    5503             : 
+    5504           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5505             : 
+    5506           0 :   if (!last_tracker_cmd) {
+    5507           0 :     res.message = "not flying";
+    5508           0 :     res.success = false;
+    5509           0 :     return true;
+    5510             :   }
+    5511             : 
+    5512           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5513           0 :   des_reference.header.frame_id      = "";
+    5514           0 :   des_reference.header.stamp         = ros::Time(0);
+    5515           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5516           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5517           0 :   des_reference.reference.position.z = req.goal;
+    5518           0 :   des_reference.reference.heading    = last_tracker_cmd->heading;
+    5519             : 
+    5520           0 :   auto [success, message] = setReference(des_reference);
+    5521             : 
+    5522           0 :   res.success = success;
+    5523           0 :   res.message = message;
+    5524             : 
+    5525           0 :   return true;
+    5526             : }
+    5527             : 
+    5528             : //}
+    5529             : 
+    5530             : /* //{ callbackSetHeading() */
+    5531             : 
+    5532           0 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5533             : 
+    5534           0 :   if (!is_initialized_) {
+    5535           0 :     res.message = "not initialized";
+    5536           0 :     res.success = false;
+    5537           0 :     return true;
+    5538             :   }
+    5539             : 
+    5540           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeading");
+    5541           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+    5542             : 
+    5543           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5544             : 
+    5545           0 :   if (!last_tracker_cmd) {
+    5546           0 :     res.message = "not flying";
+    5547           0 :     res.success = false;
+    5548           0 :     return true;
+    5549             :   }
+    5550             : 
+    5551           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5552           0 :   des_reference.header.frame_id      = "";
+    5553           0 :   des_reference.header.stamp         = ros::Time(0);
+    5554           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5555           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5556           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5557           0 :   des_reference.reference.heading    = req.goal;
+    5558             : 
+    5559           0 :   auto [success, message] = setReference(des_reference);
+    5560             : 
+    5561           0 :   res.success = success;
+    5562           0 :   res.message = message;
+    5563             : 
+    5564           0 :   return true;
+    5565             : }
+    5566             : 
+    5567             : //}
+    5568             : 
+    5569             : /* //{ callbackSetHeadingRelative() */
+    5570             : 
+    5571           0 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5572             : 
+    5573           0 :   if (!is_initialized_) {
+    5574           0 :     res.message = "not initialized";
+    5575           0 :     res.success = false;
+    5576           0 :     return true;
+    5577             :   }
+    5578             : 
+    5579           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+    5580           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+    5581             : 
+    5582           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5583             : 
+    5584           0 :   if (!last_tracker_cmd) {
+    5585           0 :     res.message = "not flying";
+    5586           0 :     res.success = false;
+    5587           0 :     return true;
+    5588             :   }
+    5589             : 
+    5590           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5591           0 :   des_reference.header.frame_id      = "";
+    5592           0 :   des_reference.header.stamp         = ros::Time(0);
+    5593           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5594           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5595           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5596           0 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal;
+    5597             : 
+    5598           0 :   auto [success, message] = setReference(des_reference);
+    5599             : 
+    5600           0 :   res.success = success;
+    5601           0 :   res.message = message;
+    5602             : 
+    5603           0 :   return true;
+    5604             : }
+    5605             : 
+    5606             : //}
+    5607             : 
+    5608             : // --------------------------------------------------------------
+    5609             : // |                          routines                          |
+    5610             : // --------------------------------------------------------------
+    5611             : 
+    5612             : /* setReference() //{ */
+    5613             : 
+    5614          50 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+    5615             : 
+    5616         100 :   std::stringstream ss;
+    5617             : 
+    5618          50 :   if (!callbacks_enabled_) {
+    5619           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5620           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5621           0 :     return std::tuple(false, ss.str());
+    5622             :   }
+    5623             : 
+    5624          50 :   if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+    5625           0 :     ss << "incoming reference is not finite!!!";
+    5626           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5627           0 :     return std::tuple(false, ss.str());
+    5628             :   }
+    5629             : 
+    5630             :   // copy member variables
+    5631         100 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5632         100 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5633             : 
+    5634             :   // transform the reference to the current frame
+    5635         100 :   auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+    5636             : 
+    5637          50 :   if (!ret) {
+    5638             : 
+    5639           0 :     ss << "the reference could not be transformed";
+    5640           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5641           0 :     return std::tuple(false, ss.str());
+    5642             :   }
+    5643             : 
+    5644         100 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5645             : 
+    5646             :   // safety area check
+    5647          50 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5648           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5649           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5650           0 :     return std::tuple(false, ss.str());
+    5651             :   }
+    5652             : 
+    5653          50 :   if (last_tracker_cmd) {
+    5654             : 
+    5655          50 :     mrs_msgs::ReferenceStamped from_point;
+    5656          50 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5657          50 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5658          50 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5659          50 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5660             : 
+    5661          50 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5662           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5663           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5664           0 :       return std::tuple(false, ss.str());
+    5665             :     }
+    5666             :   }
+    5667             : 
+    5668          50 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    5669             : 
+    5670             :   // prepare the message for current tracker
+    5671          50 :   mrs_msgs::ReferenceSrvRequest reference_request;
+    5672          50 :   reference_request.reference = transformed_reference.reference;
+    5673             : 
+    5674             :   {
+    5675         100 :     std::scoped_lock lock(mutex_tracker_list_);
+    5676             : 
+    5677          50 :     tracker_response = tracker_list_.at(active_tracker_idx_)
+    5678          50 :                            ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+    5679             : 
+    5680          50 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    5681             : 
+    5682         100 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5683             : 
+    5684             :     } else {
+    5685             : 
+    5686           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+    5687           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+    5688           0 :       return std::tuple(false, ss.str());
+    5689             :     }
+    5690             :   }
+    5691             : }
+    5692             : 
+    5693             : //}
+    5694             : 
+    5695             : /* setVelocityReference() //{ */
+    5696             : 
+    5697         720 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+    5698             : 
+    5699        1440 :   std::stringstream ss;
+    5700             : 
+    5701         720 :   if (!callbacks_enabled_) {
+    5702           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5703           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5704           0 :     return std::tuple(false, ss.str());
+    5705             :   }
+    5706             : 
+    5707         720 :   if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+    5708           0 :     ss << "velocity command is not valid!";
+    5709           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5710           0 :     return std::tuple(false, ss.str());
+    5711             :   }
+    5712             : 
+    5713             :   {
+    5714         720 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    5715             : 
+    5716         720 :     if (!last_tracker_cmd_) {
+    5717           0 :       ss << "could not set velocity command, not flying!";
+    5718           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5719           0 :       return std::tuple(false, ss.str());
+    5720             :     }
+    5721             :   }
+    5722             : 
+    5723             :   // copy member variables
+    5724        1440 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5725        1440 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5726             : 
+    5727             :   // | -- transform the velocity reference to the current frame - |
+    5728             : 
+    5729        1440 :   mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+    5730             : 
+    5731        1440 :   auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+    5732             : 
+    5733        1440 :   geometry_msgs::TransformStamped tf;
+    5734             : 
+    5735         720 :   if (!ret) {
+    5736           0 :     ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+    5737           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5738           0 :     return std::tuple(false, ss.str());
+    5739             :   } else {
+    5740         720 :     tf = ret.value();
+    5741             :   }
+    5742             : 
+    5743             :   // transform the velocity
+    5744             :   {
+    5745         720 :     geometry_msgs::Vector3Stamped velocity;
+    5746         720 :     velocity.header   = reference_in.header;
+    5747         720 :     velocity.vector.x = reference_in.reference.velocity.x;
+    5748         720 :     velocity.vector.y = reference_in.reference.velocity.y;
+    5749         720 :     velocity.vector.z = reference_in.reference.velocity.z;
+    5750             : 
+    5751         720 :     auto ret = transformer_->transform(velocity, tf);
+    5752             : 
+    5753         720 :     if (!ret) {
+    5754             : 
+    5755           0 :       ss << "the velocity reference could not be transformed";
+    5756           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5757           0 :       return std::tuple(false, ss.str());
+    5758             : 
+    5759             :     } else {
+    5760         720 :       transformed_reference.reference.velocity.x = ret->vector.x;
+    5761         720 :       transformed_reference.reference.velocity.y = ret->vector.y;
+    5762         720 :       transformed_reference.reference.velocity.z = ret->vector.z;
+    5763             :     }
+    5764             :   }
+    5765             : 
+    5766             :   // transform the z and the heading
+    5767             :   {
+    5768         720 :     geometry_msgs::PoseStamped pose;
+    5769         720 :     pose.header           = reference_in.header;
+    5770         720 :     pose.pose.position.x  = 0;
+    5771         720 :     pose.pose.position.y  = 0;
+    5772         720 :     pose.pose.position.z  = reference_in.reference.altitude;
+    5773         720 :     pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+    5774             : 
+    5775         720 :     auto ret = transformer_->transform(pose, tf);
+    5776             : 
+    5777         720 :     if (!ret) {
+    5778             : 
+    5779           0 :       ss << "the velocity reference could not be transformed";
+    5780           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5781           0 :       return std::tuple(false, ss.str());
+    5782             : 
+    5783             :     } else {
+    5784         720 :       transformed_reference.reference.altitude = ret->pose.position.z;
+    5785         720 :       transformed_reference.reference.heading  = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+    5786             :     }
+    5787             :   }
+    5788             : 
+    5789             :   // the heading rate doees not need to be transformed
+    5790         720 :   transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+    5791             : 
+    5792         720 :   transformed_reference.header.stamp    = tf.header.stamp;
+    5793         720 :   transformed_reference.header.frame_id = transformer_->frame_to(tf);
+    5794             : 
+    5795        1440 :   mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+    5796             : 
+    5797         720 :   ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+    5798             :             eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+    5799             : 
+    5800             :   // safety area check
+    5801         720 :   if (!isPointInSafetyArea3d(eqivalent_reference)) {
+    5802           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5803           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5804           0 :     return std::tuple(false, ss.str());
+    5805             :   }
+    5806             : 
+    5807         720 :   if (last_tracker_cmd) {
+    5808             : 
+    5809         720 :     mrs_msgs::ReferenceStamped from_point;
+    5810         720 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5811         720 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5812         720 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5813         720 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5814             : 
+    5815         720 :     if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+    5816           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5817           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5818           0 :       return std::tuple(false, ss.str());
+    5819             :     }
+    5820             :   }
+    5821             : 
+    5822         720 :   mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+    5823             : 
+    5824             :   // prepare the message for current tracker
+    5825         720 :   mrs_msgs::VelocityReferenceSrvRequest reference_request;
+    5826         720 :   reference_request.reference = transformed_reference.reference;
+    5827             : 
+    5828             :   {
+    5829        1440 :     std::scoped_lock lock(mutex_tracker_list_);
+    5830             : 
+    5831             :     tracker_response =
+    5832         720 :         tracker_list_.at(active_tracker_idx_)
+    5833         720 :             ->setVelocityReference(mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+    5834             : 
+    5835         720 :     if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+    5836             : 
+    5837        1440 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5838             : 
+    5839             :     } else {
+    5840             : 
+    5841           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setVelocityReference()' function!";
+    5842           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+    5843           0 :       return std::tuple(false, ss.str());
+    5844             :     }
+    5845             :   }
+    5846             : }
+    5847             : 
+    5848             : //}
+    5849             : 
+    5850             : /* setTrajectoryReference() //{ */
+    5851             : 
+    5852           6 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+    5853             :     const mrs_msgs::TrajectoryReference trajectory_in) {
+    5854             : 
+    5855          12 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5856             : 
+    5857          12 :   std::stringstream ss;
+    5858             : 
+    5859           6 :   if (!callbacks_enabled_) {
+    5860           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5861           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5862           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5863             :   }
+    5864             : 
+    5865             :   /* validate the size and check for NaNs //{ */
+    5866             : 
+    5867             :   // check for the size 0, which is invalid
+    5868           6 :   if (trajectory_in.points.size() == 0) {
+    5869             : 
+    5870           0 :     ss << "can not load trajectory with size 0";
+    5871           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5872           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5873             :   }
+    5874             : 
+    5875         643 :   for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+    5876             : 
+    5877             :     // check the point for NaN/inf
+    5878         637 :     bool valid = validateReference(trajectory_in.points.at(i), "ControlManager", "trajectory_in.points");
+    5879             : 
+    5880         637 :     if (!valid) {
+    5881             : 
+    5882           0 :       ss << "trajectory contains NaNs/infs.";
+    5883           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5884           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5885             :     }
+    5886             :   }
+    5887             : 
+    5888             :   //}
+    5889             : 
+    5890             :   /* publish the debugging topics of the original trajectory //{ */
+    5891             : 
+    5892             :   {
+    5893             : 
+    5894          12 :     geometry_msgs::PoseArray debug_trajectory_out;
+    5895           6 :     debug_trajectory_out.header = trajectory_in.header;
+    5896             : 
+    5897           6 :     debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+    5898             : 
+    5899           6 :     if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+    5900           4 :       debug_trajectory_out.header.stamp = ros::Time::now();
+    5901             :     }
+    5902             : 
+    5903         637 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5904             : 
+    5905         631 :       geometry_msgs::Pose new_pose;
+    5906             : 
+    5907         631 :       new_pose.position.x = trajectory_in.points.at(i).position.x;
+    5908         631 :       new_pose.position.y = trajectory_in.points.at(i).position.y;
+    5909         631 :       new_pose.position.z = trajectory_in.points.at(i).position.z;
+    5910             : 
+    5911         631 :       new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points.at(i).heading);
+    5912             : 
+    5913         631 :       debug_trajectory_out.poses.push_back(new_pose);
+    5914             :     }
+    5915             : 
+    5916           6 :     pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+    5917             : 
+    5918          12 :     visualization_msgs::MarkerArray msg_out;
+    5919             : 
+    5920          12 :     visualization_msgs::Marker marker;
+    5921             : 
+    5922           6 :     marker.header = trajectory_in.header;
+    5923             : 
+    5924           6 :     marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+    5925             : 
+    5926           6 :     if (marker.header.frame_id == "") {
+    5927           0 :       marker.header.frame_id = uav_state.header.frame_id;
+    5928             :     }
+    5929             : 
+    5930           6 :     if (marker.header.stamp == ros::Time(0)) {
+    5931           4 :       marker.header.stamp = ros::Time::now();
+    5932             :     }
+    5933             : 
+    5934           6 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    5935           6 :     marker.color.a          = 1;
+    5936           6 :     marker.scale.x          = 0.05;
+    5937           6 :     marker.color.r          = 0;
+    5938           6 :     marker.color.g          = 1;
+    5939           6 :     marker.color.b          = 0;
+    5940           6 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    5941             : 
+    5942         637 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5943             : 
+    5944         631 :       geometry_msgs::Point point1;
+    5945             : 
+    5946         631 :       point1.x = trajectory_in.points.at(i).position.x;
+    5947         631 :       point1.y = trajectory_in.points.at(i).position.y;
+    5948         631 :       point1.z = trajectory_in.points.at(i).position.z;
+    5949             : 
+    5950         631 :       marker.points.push_back(point1);
+    5951             : 
+    5952         631 :       geometry_msgs::Point point2;
+    5953             : 
+    5954         631 :       point2.x = trajectory_in.points.at(i + 1).position.x;
+    5955         631 :       point2.y = trajectory_in.points.at(i + 1).position.y;
+    5956         631 :       point2.z = trajectory_in.points.at(i + 1).position.z;
+    5957             : 
+    5958         631 :       marker.points.push_back(point2);
+    5959             :     }
+    5960             : 
+    5961           6 :     msg_out.markers.push_back(marker);
+    5962             : 
+    5963           6 :     pub_debug_original_trajectory_markers_.publish(msg_out);
+    5964             :   }
+    5965             : 
+    5966             :   //}
+    5967             : 
+    5968          12 :   mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+    5969             : 
+    5970           6 :   int trajectory_size = int(processed_trajectory.points.size());
+    5971             : 
+    5972           6 :   bool trajectory_modified = false;
+    5973             : 
+    5974             :   /* safety area check //{ */
+    5975             : 
+    5976           6 :   if (use_safety_area_) {
+    5977             : 
+    5978           5 :     int last_valid_idx    = 0;
+    5979           5 :     int first_invalid_idx = -1;
+    5980             : 
+    5981           5 :     double min_z = getMinZ(processed_trajectory.header.frame_id);
+    5982           5 :     double max_z = getMaxZ(processed_trajectory.header.frame_id);
+    5983             : 
+    5984         621 :     for (int i = 0; i < trajectory_size; i++) {
+    5985             : 
+    5986         616 :       if (_snap_trajectory_to_safety_area_) {
+    5987             : 
+    5988             :         // saturate the trajectory to min and max Z
+    5989           0 :         if (processed_trajectory.points.at(i).position.z < min_z) {
+    5990             : 
+    5991           0 :           processed_trajectory.points.at(i).position.z = min_z;
+    5992           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+    5993           0 :           trajectory_modified = true;
+    5994             :         }
+    5995             : 
+    5996           0 :         if (processed_trajectory.points.at(i).position.z > max_z) {
+    5997             : 
+    5998           0 :           processed_trajectory.points.at(i).position.z = max_z;
+    5999           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+    6000           0 :           trajectory_modified = true;
+    6001             :         }
+    6002             :       }
+    6003             : 
+    6004             :       // check the point against the safety area
+    6005         616 :       mrs_msgs::ReferenceStamped des_reference;
+    6006         616 :       des_reference.header    = processed_trajectory.header;
+    6007         616 :       des_reference.reference = processed_trajectory.points.at(i);
+    6008             : 
+    6009         616 :       if (!isPointInSafetyArea3d(des_reference)) {
+    6010             : 
+    6011           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+    6012           0 :         trajectory_modified = true;
+    6013             : 
+    6014             :         // the first invalid point
+    6015           0 :         if (first_invalid_idx == -1) {
+    6016             : 
+    6017           0 :           first_invalid_idx = i;
+    6018             : 
+    6019           0 :           last_valid_idx = i - 1;
+    6020             :         }
+    6021             : 
+    6022             :         // the point is ok
+    6023             :       } else {
+    6024             : 
+    6025             :         // we found a point, which is ok, after finding a point which was not ok
+    6026         616 :         if (first_invalid_idx != -1) {
+    6027             : 
+    6028             :           // special case, we had no valid point so far
+    6029           0 :           if (last_valid_idx == -1) {
+    6030             : 
+    6031           0 :             ss << "the trajectory starts outside of the safety area!";
+    6032           0 :             ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6033           0 :             return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6034             : 
+    6035             :             // we have a valid point in the past
+    6036             :           } else {
+    6037             : 
+    6038           0 :             if (!_snap_trajectory_to_safety_area_) {
+    6039           0 :               break;
+    6040             :             }
+    6041             : 
+    6042           0 :             bool interpolation_success = true;
+    6043             : 
+    6044             :             // iterpolate between the last valid point and this new valid point
+    6045           0 :             double angle = atan2((processed_trajectory.points.at(i).position.y - processed_trajectory.points.at(last_valid_idx).position.y),
+    6046           0 :                                  (processed_trajectory.points.at(i).position.x - processed_trajectory.points.at(last_valid_idx).position.x));
+    6047             : 
+    6048           0 :             double dist_two_points = mrs_lib::geometry::dist(
+    6049           0 :                 vec2_t(processed_trajectory.points.at(i).position.x, processed_trajectory.points.at(i).position.y),
+    6050           0 :                 vec2_t(processed_trajectory.points.at(last_valid_idx).position.x, processed_trajectory.points.at(last_valid_idx).position.y));
+    6051           0 :             double step = dist_two_points / (i - last_valid_idx);
+    6052             : 
+    6053           0 :             for (int j = last_valid_idx; j < i; j++) {
+    6054             : 
+    6055           0 :               mrs_msgs::ReferenceStamped temp_point;
+    6056           0 :               temp_point.header.frame_id      = processed_trajectory.header.frame_id;
+    6057           0 :               temp_point.reference.position.x = processed_trajectory.points.at(last_valid_idx).position.x + (j - last_valid_idx) * cos(angle) * step;
+    6058           0 :               temp_point.reference.position.y = processed_trajectory.points.at(last_valid_idx).position.y + (j - last_valid_idx) * sin(angle) * step;
+    6059             : 
+    6060           0 :               if (!isPointInSafetyArea2d(temp_point)) {
+    6061             : 
+    6062           0 :                 interpolation_success = false;
+    6063           0 :                 break;
+    6064             : 
+    6065             :               } else {
+    6066             : 
+    6067           0 :                 processed_trajectory.points.at(j).position.x = temp_point.reference.position.x;
+    6068           0 :                 processed_trajectory.points.at(j).position.y = temp_point.reference.position.y;
+    6069             :               }
+    6070             :             }
+    6071             : 
+    6072           0 :             if (!interpolation_success) {
+    6073           0 :               break;
+    6074             :             }
+    6075             :           }
+    6076             : 
+    6077           0 :           first_invalid_idx = -1;
+    6078             :         }
+    6079             :       }
+    6080             :     }
+    6081             : 
+    6082             :     // special case, the trajectory does not end with a valid point
+    6083           5 :     if (first_invalid_idx != -1) {
+    6084             : 
+    6085             :       // super special case, the whole trajectory is invalid
+    6086           0 :       if (first_invalid_idx == 0) {
+    6087             : 
+    6088           0 :         ss << "the whole trajectory is outside of the safety area!";
+    6089           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6090           0 :         return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6091             : 
+    6092             :         // there is a good portion of the trajectory in the beginning
+    6093             :       } else {
+    6094             : 
+    6095           0 :         trajectory_size = last_valid_idx + 1;
+    6096           0 :         processed_trajectory.points.resize(trajectory_size);
+    6097           0 :         trajectory_modified = true;
+    6098             :       }
+    6099             :     }
+    6100             :   }
+    6101             : 
+    6102           6 :   if (trajectory_size == 0) {
+    6103             : 
+    6104           0 :     ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+    6105           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6106           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6107             :   }
+    6108             : 
+    6109             :   //}
+    6110             : 
+    6111             :   /* transform the trajectory to the current control frame //{ */
+    6112             : 
+    6113           6 :   std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+    6114             : 
+    6115           6 :   if (processed_trajectory.header.stamp > ros::Time::now()) {
+    6116           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+    6117             :   } else {
+    6118           6 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+    6119             :   }
+    6120             : 
+    6121           6 :   if (!tf_traj_state) {
+    6122           0 :     ss << "could not create TF transformer for the trajectory";
+    6123           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6124           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6125             :   }
+    6126             : 
+    6127           6 :   processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    6128             : 
+    6129         643 :   for (int i = 0; i < trajectory_size; i++) {
+    6130             : 
+    6131         637 :     mrs_msgs::ReferenceStamped trajectory_point;
+    6132         637 :     trajectory_point.header    = processed_trajectory.header;
+    6133         637 :     trajectory_point.reference = processed_trajectory.points.at(i);
+    6134             : 
+    6135         637 :     auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    6136             : 
+    6137         637 :     if (!ret) {
+    6138             : 
+    6139           0 :       ss << "trajectory cannnot be transformed";
+    6140           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6141           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6142             : 
+    6143             :     } else {
+    6144             : 
+    6145             :       // transform the points in the trajectory to the current frame
+    6146         637 :       processed_trajectory.points.at(i) = ret.value().reference;
+    6147             :     }
+    6148             :   }
+    6149             : 
+    6150             :   //}
+    6151             : 
+    6152           6 :   mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+    6153          12 :   mrs_msgs::TrajectoryReferenceSrvRequest            request;
+    6154             : 
+    6155             :   // check for empty trajectory
+    6156           6 :   if (processed_trajectory.points.size() == 0) {
+    6157           0 :     ss << "reference trajectory was processing and it is now empty, this should not happen!";
+    6158           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6159           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6160             :   }
+    6161             : 
+    6162             :   // prepare the message for current tracker
+    6163           6 :   request.trajectory = processed_trajectory;
+    6164             : 
+    6165             :   bool                     success;
+    6166          12 :   std::string              message;
+    6167             :   bool                     modified;
+    6168          12 :   std::vector<std::string> tracker_names;
+    6169          12 :   std::vector<bool>        tracker_successes;
+    6170          12 :   std::vector<std::string> tracker_messages;
+    6171             : 
+    6172             :   {
+    6173          12 :     std::scoped_lock lock(mutex_tracker_list_);
+    6174             : 
+    6175             :     // set the trajectory to the currently active tracker
+    6176             :     response =
+    6177           6 :         tracker_list_.at(active_tracker_idx_)
+    6178           6 :             ->setTrajectoryReference(mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6179             : 
+    6180           6 :     tracker_names.push_back(_tracker_names_.at(active_tracker_idx_));
+    6181             : 
+    6182           6 :     if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6183             : 
+    6184           5 :       success  = response->success;
+    6185           5 :       message  = response->message;
+    6186           5 :       modified = response->modified || trajectory_modified;
+    6187           5 :       tracker_successes.push_back(response->success);
+    6188           5 :       tracker_messages.push_back(response->message);
+    6189             : 
+    6190             :     } else {
+    6191             : 
+    6192           1 :       ss << "the active tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setTrajectoryReference()' function!";
+    6193           1 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6194             : 
+    6195           1 :       success  = true;
+    6196           1 :       message  = ss.str();
+    6197           1 :       modified = false;
+    6198           1 :       tracker_successes.push_back(false);
+    6199           1 :       tracker_messages.push_back(ss.str());
+    6200             :     }
+    6201             : 
+    6202             :     // set the trajectory to the non-active trackers
+    6203          42 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6204             : 
+    6205          36 :       if (i != active_tracker_idx_) {
+    6206             : 
+    6207          30 :         tracker_names.push_back(_tracker_names_.at(i));
+    6208             : 
+    6209          90 :         response = tracker_list_.at(i)->setTrajectoryReference(
+    6210          90 :             mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6211             : 
+    6212          30 :         if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6213             : 
+    6214           1 :           tracker_successes.push_back(response->success);
+    6215           1 :           tracker_messages.push_back(response->message);
+    6216             : 
+    6217           1 :           if (response->success) {
+    6218           2 :             std::stringstream ss;
+    6219           1 :             ss << "trajectory loaded to non-active tracker '" << _tracker_names_.at(i);
+    6220           1 :             ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6221             :           }
+    6222             : 
+    6223             :         } else {
+    6224             : 
+    6225          29 :           std::stringstream ss;
+    6226          29 :           ss << "the tracker \"" << _tracker_names_.at(i) << "\" does not implement setTrajectoryReference()";
+    6227          29 :           tracker_successes.push_back(false);
+    6228          29 :           tracker_messages.push_back(ss.str());
+    6229             :         }
+    6230             :       }
+    6231             :     }
+    6232             :   }
+    6233             : 
+    6234           6 :   return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+    6235             : }
+    6236             : 
+    6237             : //}
+    6238             : 
+    6239             : /* isOffboard() //{ */
+    6240             : 
+    6241          18 : bool ControlManager::isOffboard(void) {
+    6242             : 
+    6243          18 :   if (!sh_hw_api_status_.hasMsg()) {
+    6244           0 :     return false;
+    6245             :   }
+    6246             : 
+    6247          18 :   mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+    6248             : 
+    6249          18 :   return hw_api_status->connected && hw_api_status->offboard;
+    6250             : }
+    6251             : 
+    6252             : //}
+    6253             : 
+    6254             : /* setCallbacks() //{ */
+    6255             : 
+    6256          94 : void ControlManager::setCallbacks(bool in) {
+    6257             : 
+    6258          94 :   callbacks_enabled_ = in;
+    6259             : 
+    6260          94 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    6261          94 :   req_enable_callbacks.data = callbacks_enabled_;
+    6262             : 
+    6263             :   {
+    6264         188 :     std::scoped_lock lock(mutex_tracker_list_);
+    6265             : 
+    6266             :     // set callbacks to all trackers
+    6267         658 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6268         564 :       tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6269             :     }
+    6270             :   }
+    6271          94 : }
+    6272             : 
+    6273             : //}
+    6274             : 
+    6275             : /* publishDiagnostics() //{ */
+    6276             : 
+    6277       17256 : void ControlManager::publishDiagnostics(void) {
+    6278             : 
+    6279       17256 :   if (!is_initialized_) {
+    6280           0 :     return;
+    6281             :   }
+    6282             : 
+    6283       51768 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publishDiagnostics");
+    6284       51768 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    6285             : 
+    6286       34512 :   std::scoped_lock lock(mutex_diagnostics_);
+    6287             : 
+    6288       34512 :   mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+    6289             : 
+    6290       17256 :   diagnostics_msg.stamp    = ros::Time::now();
+    6291       17256 :   diagnostics_msg.uav_name = _uav_name_;
+    6292             : 
+    6293       17256 :   diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+    6294             : 
+    6295       17256 :   diagnostics_msg.output_enabled = output_enabled_;
+    6296             : 
+    6297       17256 :   diagnostics_msg.joystick_active = rc_goto_active_;
+    6298             : 
+    6299             :   {
+    6300       17256 :     std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+    6301             : 
+    6302       17256 :     diagnostics_msg.flying_normally = isFlyingNormally();
+    6303             :   }
+    6304             : 
+    6305       17256 :   diagnostics_msg.bumper_active = bumper_repulsing_;
+    6306             : 
+    6307             :   // | ----------------- fill the tracker status ---------------- |
+    6308             : 
+    6309             :   {
+    6310       34512 :     std::scoped_lock lock(mutex_tracker_list_);
+    6311             : 
+    6312       17256 :     mrs_msgs::TrackerStatus tracker_status;
+    6313             : 
+    6314       17256 :     diagnostics_msg.active_tracker = _tracker_names_.at(active_tracker_idx_);
+    6315       17256 :     diagnostics_msg.tracker_status = tracker_list_.at(active_tracker_idx_)->getStatus();
+    6316             :   }
+    6317             : 
+    6318             :   // | --------------- fill the controller status --------------- |
+    6319             : 
+    6320             :   {
+    6321       34512 :     std::scoped_lock lock(mutex_controller_list_);
+    6322             : 
+    6323       17256 :     mrs_msgs::ControllerStatus controller_status;
+    6324             : 
+    6325       17256 :     diagnostics_msg.active_controller = _controller_names_.at(active_controller_idx_);
+    6326       17256 :     diagnostics_msg.controller_status = controller_list_.at(active_controller_idx_)->getStatus();
+    6327             :   }
+    6328             : 
+    6329             :   // | ------------ fill in the available controllers ----------- |
+    6330             : 
+    6331      103536 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    6332       86280 :     if ((_controller_names_.at(i) != _failsafe_controller_name_) && (_controller_names_.at(i) != _eland_controller_name_)) {
+    6333       51768 :       diagnostics_msg.available_controllers.push_back(_controller_names_.at(i));
+    6334       51768 :       diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_.at(i)).human_switchable);
+    6335             :     }
+    6336             :   }
+    6337             : 
+    6338             :   // | ------------- fill in the available trackers ------------- |
+    6339             : 
+    6340      121014 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    6341      103758 :     if (_tracker_names_.at(i) != _null_tracker_name_) {
+    6342       86502 :       diagnostics_msg.available_trackers.push_back(_tracker_names_.at(i));
+    6343       86502 :       diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_.at(i)).human_switchable);
+    6344             :     }
+    6345             :   }
+    6346             : 
+    6347             :   // | ------------------------- publish ------------------------ |
+    6348             : 
+    6349       17256 :   ph_diagnostics_.publish(diagnostics_msg);
+    6350             : }
+    6351             : 
+    6352             : //}
+    6353             : 
+    6354             : /* setConstraintsToTrackers() //{ */
+    6355             : 
+    6356         325 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6357             : 
+    6358         975 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+    6359         975 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+    6360             : 
+    6361         325 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6362             : 
+    6363             :   {
+    6364         650 :     std::scoped_lock lock(mutex_tracker_list_);
+    6365             : 
+    6366             :     // for each tracker
+    6367        2278 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6368             : 
+    6369             :       // if it is the active one, update and retrieve the command
+    6370        5859 :       response = tracker_list_.at(i)->setConstraints(
+    6371        5859 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6372             :     }
+    6373             :   }
+    6374         325 : }
+    6375             : 
+    6376             : //}
+    6377             : 
+    6378             : /* setConstraintsToControllers() //{ */
+    6379             : 
+    6380         372 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6381             : 
+    6382        1116 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+    6383        1116 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+    6384             : 
+    6385         372 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6386             : 
+    6387             :   {
+    6388         744 :     std::scoped_lock lock(mutex_controller_list_);
+    6389             : 
+    6390             :     // for each controller
+    6391        2232 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    6392             : 
+    6393             :       // if it is the active one, update and retrieve the command
+    6394        5580 :       response = controller_list_.at(i)->setConstraints(
+    6395        5580 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6396             :     }
+    6397             :   }
+    6398         372 : }
+    6399             : 
+    6400             : //}
+    6401             : 
+    6402             : /* setConstraints() //{ */
+    6403             : 
+    6404          94 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6405             : 
+    6406         282 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraints");
+    6407         282 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+    6408             : 
+    6409          94 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6410             : 
+    6411          94 :   setConstraintsToTrackers(constraints);
+    6412             : 
+    6413          94 :   setConstraintsToControllers(constraints);
+    6414          94 : }
+    6415             : 
+    6416             : //}
+    6417             : 
+    6418             : /* enforceControllerConstraints() //{ */
+    6419             : 
+    6420      126575 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+    6421             :     const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6422             : 
+    6423             :   // copy member variables
+    6424      253150 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6425             : 
+    6426      126575 :   if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+    6427      107355 :     return {};
+    6428             :   }
+    6429             : 
+    6430       19220 :   bool enforcing = false;
+    6431             : 
+    6432       19220 :   auto constraints_out = constraints;
+    6433             : 
+    6434       38440 :   std::scoped_lock lock(mutex_tracker_list_);
+    6435             : 
+    6436             :   // enforce horizontal speed
+    6437       19220 :   if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+    6438       13921 :     constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+    6439             : 
+    6440       13921 :     enforcing = true;
+    6441             :   }
+    6442             : 
+    6443             :   // enforce horizontal acceleration
+    6444       19220 :   if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+    6445       17574 :     constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+    6446             : 
+    6447       17574 :     enforcing = true;
+    6448             :   }
+    6449             : 
+    6450             :   // enforce vertical ascending speed
+    6451       19220 :   if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+    6452       13921 :     constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+    6453             : 
+    6454       13921 :     enforcing = true;
+    6455             :   }
+    6456             : 
+    6457             :   // enforce vertical ascending acceleration
+    6458       19220 :   if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+    6459           0 :     constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+    6460             : 
+    6461           0 :     enforcing = true;
+    6462             :   }
+    6463             : 
+    6464             :   // enforce vertical descending speed
+    6465       19220 :   if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+    6466       13921 :     constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+    6467             : 
+    6468       13921 :     enforcing = true;
+    6469             :   }
+    6470             : 
+    6471             :   // enforce vertical descending acceleration
+    6472       19220 :   if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+    6473           0 :     constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+    6474             : 
+    6475           0 :     enforcing = true;
+    6476             :   }
+    6477             : 
+    6478       19220 :   if (enforcing) {
+    6479       17574 :     return {constraints_out};
+    6480             :   } else {
+    6481        1646 :     return {};
+    6482             :   }
+    6483             : }
+    6484             : 
+    6485             : //}
+    6486             : 
+    6487             : /* isFlyingNormally() //{ */
+    6488             : 
+    6489       17522 : bool ControlManager::isFlyingNormally(void) {
+    6490             : 
+    6491       14863 :   return callbacks_enabled_ && (output_enabled_) && (offboard_mode_) && (armed_) &&
+    6492        9347 :          (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+    6493        9347 :            (active_controller_idx_ != _failsafe_controller_idx_)) ||
+    6494       34581 :           _controller_names_.size() == 1) &&
+    6495       24673 :          (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+    6496             : }
+    6497             : 
+    6498             : //}
+    6499             : 
+    6500             : /* //{ getMass() */
+    6501             : 
+    6502         486 : double ControlManager::getMass(void) {
+    6503             : 
+    6504         972 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6505             : 
+    6506         486 :   if (last_control_output.diagnostics.mass_estimator) {
+    6507          14 :     return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    6508             :   } else {
+    6509         472 :     return _uav_mass_;
+    6510             :   }
+    6511             : }
+    6512             : 
+    6513             : //}
+    6514             : 
+    6515             : /* loadConfigFile() //{ */
+    6516             : 
+    6517           0 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+    6518             : 
+    6519           0 :   const std::string name_space = nh_.getNamespace() + "/" + ns;
+    6520             : 
+    6521           0 :   ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+    6522             : 
+    6523             :   // load the user-requested file
+    6524             :   {
+    6525           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    6526           0 :     int         result  = std::system(command.c_str());
+    6527             : 
+    6528           0 :     if (result != 0) {
+    6529           0 :       ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+    6530           0 :       return false;
+    6531             :     }
+    6532             :   }
+    6533             : 
+    6534             :   // load the platform config
+    6535           0 :   if (_platform_config_ != "") {
+    6536           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    6537           0 :     int         result  = std::system(command.c_str());
+    6538             : 
+    6539           0 :     if (result != 0) {
+    6540           0 :       ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+    6541           0 :       return false;
+    6542             :     }
+    6543             :   }
+    6544             : 
+    6545             :   // load the custom config
+    6546           0 :   if (_custom_config_ != "") {
+    6547           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    6548           0 :     int         result  = std::system(command.c_str());
+    6549             : 
+    6550           0 :     if (result != 0) {
+    6551           0 :       ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+    6552           0 :       return false;
+    6553             :     }
+    6554             :   }
+    6555             : 
+    6556           0 :   return true;
+    6557             : }
+    6558             : 
+    6559             : //}
+    6560             : 
+    6561             : // | ----------------------- safety area ---------------------- |
+    6562             : 
+    6563             : /* //{ isPointInSafetyArea3d() */
+    6564             : 
+    6565        1630 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+    6566             : 
+    6567        1630 :   if (!use_safety_area_) {
+    6568         648 :     return true;
+    6569             :   }
+    6570             : 
+    6571        1964 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6572             : 
+    6573         982 :   if (!tfed_horizontal) {
+    6574           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6575           0 :     return false;
+    6576             :   }
+    6577             : 
+    6578         982 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6579           0 :     return false;
+    6580             :   }
+    6581             : 
+    6582         982 :   if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+    6583           0 :     return false;
+    6584             :   }
+    6585             : 
+    6586         982 :   return true;
+    6587             : }
+    6588             : 
+    6589             : //}
+    6590             : 
+    6591             : /* //{ isPointInSafetyArea2d() */
+    6592             : 
+    6593        9946 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+    6594             : 
+    6595        9946 :   if (!use_safety_area_) {
+    6596         589 :     return true;
+    6597             :   }
+    6598             : 
+    6599       18714 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6600             : 
+    6601        9357 :   if (!tfed_horizontal) {
+    6602           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6603           0 :     return false;
+    6604             :   }
+    6605             : 
+    6606        9357 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6607          78 :     return false;
+    6608             :   }
+    6609             : 
+    6610        9279 :   return true;
+    6611             : }
+    6612             : 
+    6613             : //}
+    6614             : 
+    6615             : /* //{ isPathToPointInSafetyArea3d() */
+    6616             : 
+    6617         770 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6618             : 
+    6619         770 :   if (!use_safety_area_) {
+    6620         648 :     return true;
+    6621             :   }
+    6622             : 
+    6623         122 :   if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+    6624           0 :     return false;
+    6625             :   }
+    6626             : 
+    6627         244 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6628             : 
+    6629             :   {
+    6630         122 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6631             : 
+    6632         122 :     if (!ret) {
+    6633             : 
+    6634           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6635             : 
+    6636           0 :       return false;
+    6637             :     }
+    6638             : 
+    6639         122 :     start_transformed = ret.value();
+    6640             :   }
+    6641             : 
+    6642             :   {
+    6643         122 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6644             : 
+    6645         122 :     if (!ret) {
+    6646             : 
+    6647           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6648             : 
+    6649           0 :       return false;
+    6650             :     }
+    6651             : 
+    6652         122 :     end_transformed = ret.value();
+    6653             :   }
+    6654             : 
+    6655         122 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6656         122 :                                    end_transformed.reference.position.y);
+    6657             : }
+    6658             : 
+    6659             : //}
+    6660             : 
+    6661             : /* //{ isPathToPointInSafetyArea2d() */
+    6662             : 
+    6663          31 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6664             : 
+    6665          31 :   if (!use_safety_area_) {
+    6666           0 :     return true;
+    6667             :   }
+    6668             : 
+    6669          62 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6670             : 
+    6671          31 :   if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+    6672           0 :     return false;
+    6673             :   }
+    6674             : 
+    6675             :   {
+    6676          31 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6677             : 
+    6678          31 :     if (!ret) {
+    6679             : 
+    6680           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6681             : 
+    6682           0 :       return false;
+    6683             :     }
+    6684             : 
+    6685          31 :     start_transformed = ret.value();
+    6686             :   }
+    6687             : 
+    6688             :   {
+    6689          31 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6690             : 
+    6691          31 :     if (!ret) {
+    6692             : 
+    6693           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6694             : 
+    6695           0 :       return false;
+    6696             :     }
+    6697             : 
+    6698          31 :     end_transformed = ret.value();
+    6699             :   }
+    6700             : 
+    6701          31 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6702          31 :                                    end_transformed.reference.position.y);
+    6703             : }
+    6704             : 
+    6705             : //}
+    6706             : 
+    6707             : /* //{ getMaxZ() */
+    6708             : 
+    6709       12245 : double ControlManager::getMaxZ(const std::string& frame_id) {
+    6710             : 
+    6711             :   // | ------- first, calculate max_z from the safety area ------ |
+    6712             : 
+    6713       12245 :   double safety_area_max_z = std::numeric_limits<float>::max();
+    6714             : 
+    6715             :   {
+    6716             : 
+    6717       24490 :     geometry_msgs::PointStamped point;
+    6718             : 
+    6719       12245 :     point.header.frame_id = _safety_area_vertical_frame_;
+    6720       12245 :     point.point.x         = 0;
+    6721       12245 :     point.point.y         = 0;
+    6722       12245 :     point.point.z         = _safety_area_max_z_;
+    6723             : 
+    6724       12245 :     auto ret = transformer_->transformSingle(point, frame_id);
+    6725             : 
+    6726       12245 :     if (!ret) {
+    6727           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+    6728             :     }
+    6729             : 
+    6730       12245 :     safety_area_max_z = ret->point.z;
+    6731             :   }
+    6732             : 
+    6733             :   // | ------------ overwrite from estimation manager ----------- |
+    6734             : 
+    6735       12245 :   double estimation_manager_max_z = std::numeric_limits<float>::max();
+    6736             : 
+    6737             :   {
+    6738             :     // if possible, override it with max z from the estimation manager
+    6739       12245 :     if (sh_max_z_.hasMsg()) {
+    6740             : 
+    6741       24456 :       auto msg = sh_max_z_.getMsg();
+    6742             : 
+    6743             :       // transform it into the safety area frame
+    6744       24456 :       geometry_msgs::PointStamped point;
+    6745       12228 :       point.header  = msg->header;
+    6746       12228 :       point.point.x = 0;
+    6747       12228 :       point.point.y = 0;
+    6748       12228 :       point.point.z = msg->value;
+    6749             : 
+    6750       12228 :       auto ret = transformer_->transformSingle(point, frame_id);
+    6751             : 
+    6752       12228 :       if (!ret) {
+    6753           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+    6754             :       }
+    6755             : 
+    6756       12228 :       estimation_manager_max_z = ret->point.z;
+    6757             :     }
+    6758             :   }
+    6759             : 
+    6760       12245 :   if (estimation_manager_max_z < safety_area_max_z) {
+    6761       11446 :     return estimation_manager_max_z;
+    6762             :   } else {
+    6763         799 :     return safety_area_max_z;
+    6764             :   }
+    6765             : }
+    6766             : 
+    6767             : //}
+    6768             : 
+    6769             : /* //{ getMinZ() */
+    6770             : 
+    6771       12245 : double ControlManager::getMinZ(const std::string& frame_id) {
+    6772             : 
+    6773       12245 :   if (!use_safety_area_) {
+    6774           0 :     return std::numeric_limits<double>::lowest();
+    6775             :   }
+    6776             : 
+    6777       24490 :   geometry_msgs::PointStamped point;
+    6778             : 
+    6779       12245 :   point.header.frame_id = _safety_area_vertical_frame_;
+    6780       12245 :   point.point.x         = 0;
+    6781       12245 :   point.point.y         = 0;
+    6782       12245 :   point.point.z         = _safety_area_min_z_;
+    6783             : 
+    6784       24490 :   auto ret = transformer_->transformSingle(point, frame_id);
+    6785             : 
+    6786       12245 :   if (!ret) {
+    6787           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+    6788           0 :     return std::numeric_limits<double>::lowest();
+    6789             :   }
+    6790             : 
+    6791       12245 :   return ret->point.z;
+    6792             : }
+    6793             : 
+    6794             : //}
+    6795             : 
+    6796             : // | --------------------- obstacle bumper -------------------- |
+    6797             : 
+    6798             : /* bumperPushFromObstacle() //{ */
+    6799             : 
+    6800         260 : void ControlManager::bumperPushFromObstacle(void) {
+    6801             : 
+    6802             :   // | --------------- fabricate the min distances -------------- |
+    6803             : 
+    6804         260 :   double min_distance_horizontal = _bumper_horizontal_distance_;
+    6805         260 :   double min_distance_vertical   = _bumper_vertical_distance_;
+    6806             : 
+    6807         260 :   if (_bumper_horizontal_derive_from_dynamics_ || _bumper_vertical_derive_from_dynamics_) {
+    6808             : 
+    6809         260 :     auto constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    6810             : 
+    6811         260 :     if (_bumper_horizontal_derive_from_dynamics_) {
+    6812             : 
+    6813         260 :       const double horizontal_t_stop    = constraints.constraints.horizontal_speed / constraints.constraints.horizontal_acceleration;
+    6814         260 :       const double horizontal_stop_dist = (horizontal_t_stop * constraints.constraints.horizontal_speed) / 2.0;
+    6815             : 
+    6816         260 :       min_distance_horizontal += 1.5 * horizontal_stop_dist;
+    6817             :     }
+    6818             : 
+    6819         260 :     if (_bumper_vertical_derive_from_dynamics_) {
+    6820             : 
+    6821             : 
+    6822             :       // larger from the two accelerations
+    6823         520 :       const double vert_acc = constraints.constraints.vertical_ascending_acceleration > constraints.constraints.vertical_descending_acceleration
+    6824         260 :                                   ? constraints.constraints.vertical_ascending_acceleration
+    6825             :                                   : constraints.constraints.vertical_descending_acceleration;
+    6826             : 
+    6827             :       // larger from the two speeds
+    6828         520 :       const double vert_speed = constraints.constraints.vertical_ascending_speed > constraints.constraints.vertical_descending_speed
+    6829         260 :                                     ? constraints.constraints.vertical_ascending_speed
+    6830             :                                     : constraints.constraints.vertical_descending_speed;
+    6831             : 
+    6832         260 :       const double vertical_t_stop    = vert_speed / vert_acc;
+    6833         260 :       const double vertical_stop_dist = (vertical_t_stop * vert_speed) / 2.0;
+    6834             : 
+    6835         260 :       min_distance_vertical += 1.5 * vertical_stop_dist;
+    6836             :     }
+    6837             :   }
+    6838             : 
+    6839             :   // | ----------------------------  ---------------------------- |
+    6840             : 
+    6841             :   // copy member variables
+    6842         260 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6843         260 :   auto                              uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    6844             : 
+    6845         260 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    6846             : 
+    6847         260 :   double direction          = 0;
+    6848         260 :   double repulsion_distance = std::numeric_limits<double>::max();
+    6849             : 
+    6850         260 :   bool horizontal_collision_detected = false;
+    6851         260 :   bool vertical_collision_detected   = false;
+    6852             : 
+    6853         260 :   double min_horizontal_sector_distance = std::numeric_limits<double>::max();
+    6854         260 :   size_t min_sector_id                  = 0;
+    6855             : 
+    6856        2340 :   for (unsigned long i = 0; i < bumper_data->n_horizontal_sectors; i++) {
+    6857             : 
+    6858        2080 :     if (bumper_data->sectors.at(i) < 0) {
+    6859           0 :       continue;
+    6860             :     }
+    6861             : 
+    6862        2080 :     if (bumper_data->sectors.at(i) < min_horizontal_sector_distance) {
+    6863         260 :       min_horizontal_sector_distance = bumper_data->sectors.at(i);
+    6864         260 :       min_sector_id                  = i;
+    6865             :     }
+    6866             :   }
+    6867             : 
+    6868             :   // if the sector is under the threshold distance
+    6869         260 :   if (min_horizontal_sector_distance < min_distance_horizontal) {
+    6870             : 
+    6871             :     // get the desired direction of motion
+    6872         104 :     double oposite_direction  = double(min_sector_id) * sector_size + M_PI;
+    6873         104 :     int    oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+    6874             : 
+    6875             :     // get the id of the oposite sector
+    6876         104 :     direction = oposite_direction;
+    6877             : 
+    6878         104 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %lu vs. %d), obstacle distance: %.2f, repulsing", min_sector_id,
+    6879             :                       oposite_sector_idx, bumper_data->sectors.at(min_sector_id));
+    6880             : 
+    6881         104 :     repulsion_distance = min_distance_horizontal + _bumper_horizontal_overshoot_ - bumper_data->sectors.at(min_sector_id);
+    6882             : 
+    6883         104 :     horizontal_collision_detected = true;
+    6884             :   }
+    6885             : 
+    6886         260 :   double vertical_repulsion_distance = 0;
+    6887             : 
+    6888             :   // check for vertical collision down
+    6889         260 :   if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors) > 0 && bumper_data->sectors.at(bumper_data->n_horizontal_sectors) <= min_distance_vertical) {
+    6890             : 
+    6891           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+    6892           0 :     vertical_collision_detected = true;
+    6893           0 :     vertical_repulsion_distance = min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors);
+    6894             :   }
+    6895             : 
+    6896             :   // check for vertical collision up
+    6897         520 :   if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) > 0 &&
+    6898         260 :       bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) <= min_distance_vertical) {
+    6899             : 
+    6900           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+    6901           0 :     vertical_collision_detected = true;
+    6902           0 :     vertical_repulsion_distance = -(min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1));
+    6903             :   }
+    6904             : 
+    6905             :   // if potential collision was detected and we should start the repulsing_
+    6906         260 :   if (horizontal_collision_detected || vertical_collision_detected) {
+    6907             : 
+    6908         104 :     if (!bumper_repulsing_) {
+    6909             : 
+    6910           1 :       if (_bumper_switch_tracker_) {
+    6911             : 
+    6912           1 :         auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6913           2 :         std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+    6914             : 
+    6915             :         // remember the previously active tracker
+    6916           1 :         bumper_previous_tracker_ = active_tracker_name;
+    6917             : 
+    6918           1 :         if (active_tracker_name != _bumper_tracker_name_) {
+    6919             : 
+    6920           0 :           switchTracker(_bumper_tracker_name_);
+    6921             :         }
+    6922             :       }
+    6923             : 
+    6924           1 :       if (_bumper_switch_controller_) {
+    6925             : 
+    6926           1 :         auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6927           2 :         std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    6928             : 
+    6929             :         // remember the previously active controller
+    6930           1 :         bumper_previous_controller_ = active_controller_name;
+    6931             : 
+    6932           1 :         if (active_controller_name != _bumper_controller_name_) {
+    6933             : 
+    6934           1 :           switchController(_bumper_controller_name_);
+    6935             :         }
+    6936             :       }
+    6937             :     }
+    6938             : 
+    6939         104 :     bumper_repulsing_ = true;
+    6940             : 
+    6941         104 :     callbacks_enabled_ = false;
+    6942             : 
+    6943           0 :     mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    6944             : 
+    6945         104 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6946             : 
+    6947             :     // create the reference in the fcu_untilted frame
+    6948         104 :     mrs_msgs::ReferenceStamped reference_fcu_untilted;
+    6949             : 
+    6950         104 :     reference_fcu_untilted.header.frame_id = "fcu_untilted";
+    6951             : 
+    6952         104 :     if (horizontal_collision_detected) {
+    6953         104 :       reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+    6954         104 :       reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+    6955             :     } else {
+    6956           0 :       reference_fcu_untilted.reference.position.x = 0;
+    6957           0 :       reference_fcu_untilted.reference.position.y = 0;
+    6958             :     }
+    6959             : 
+    6960         104 :     reference_fcu_untilted.reference.heading = 0;
+    6961             : 
+    6962         104 :     if (vertical_collision_detected) {
+    6963           0 :       reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+    6964             :     } else {
+    6965         104 :       reference_fcu_untilted.reference.position.z = 0;
+    6966             :     }
+    6967             : 
+    6968             :     {
+    6969         104 :       std::scoped_lock lock(mutex_tracker_list_);
+    6970             : 
+    6971             :       // transform the reference into the currently used frame
+    6972             :       // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+    6973             :       // to the tracker before we actually call the goto service
+    6974             : 
+    6975         104 :       auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+    6976             : 
+    6977         104 :       if (!ret) {
+    6978           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+    6979           0 :         return;
+    6980             :       }
+    6981             : 
+    6982         104 :       reference_fcu_untilted = ret.value();
+    6983             : 
+    6984             :       // copy the reference into the service type message
+    6985         104 :       mrs_msgs::ReferenceSrvRequest req_goto_out;
+    6986         104 :       req_goto_out.reference = reference_fcu_untilted.reference;
+    6987             : 
+    6988             :       // disable callbacks of all trackers
+    6989         104 :       req_enable_callbacks.data = false;
+    6990         728 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6991         624 :         tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6992             :       }
+    6993             : 
+    6994             :       // enable the callbacks for the active tracker
+    6995         104 :       req_enable_callbacks.data = true;
+    6996         104 :       tracker_list_.at(active_tracker_idx_)
+    6997         104 :           ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6998             : 
+    6999             :       // call the goto
+    7000         104 :       tracker_response = tracker_list_.at(active_tracker_idx_)
+    7001         104 :                              ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    7002             : 
+    7003             :       // disable the callbacks back again
+    7004         104 :       req_enable_callbacks.data = false;
+    7005         104 :       tracker_list_.at(active_tracker_idx_)
+    7006         104 :           ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7007             :     }
+    7008             :   }
+    7009             : 
+    7010             :   // if repulsing_ and the distance is safe once again
+    7011         260 :   if (bumper_repulsing_ && !horizontal_collision_detected && !vertical_collision_detected) {
+    7012             : 
+    7013           1 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: no more collision, stopping repulsion");
+    7014             : 
+    7015           1 :     if (_bumper_switch_tracker_) {
+    7016             : 
+    7017           1 :       auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7018           2 :       std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+    7019             : 
+    7020           1 :       if (active_tracker_name != bumper_previous_tracker_) {
+    7021             : 
+    7022           0 :         switchTracker(bumper_previous_tracker_);
+    7023             :       }
+    7024             :     }
+    7025             : 
+    7026           1 :     if (_bumper_switch_controller_) {
+    7027             : 
+    7028           1 :       auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7029           2 :       std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    7030             : 
+    7031           1 :       if (active_controller_name != bumper_previous_controller_) {
+    7032             : 
+    7033           1 :         switchController(bumper_previous_controller_);
+    7034             :       }
+    7035             :     }
+    7036             : 
+    7037           1 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    7038             : 
+    7039             :     {
+    7040           2 :       std::scoped_lock lock(mutex_tracker_list_);
+    7041             : 
+    7042             :       // enable callbacks of all trackers
+    7043           1 :       req_enable_callbacks.data = true;
+    7044           7 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    7045           6 :         tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7046             :       }
+    7047             :     }
+    7048             : 
+    7049           1 :     callbacks_enabled_ = true;
+    7050             : 
+    7051           1 :     bumper_repulsing_ = false;
+    7052             :   }
+    7053             : }
+    7054             : 
+    7055             : //}
+    7056             : 
+    7057             : /* bumperGetSectorId() //{ */
+    7058             : 
+    7059         104 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+    7060             : 
+    7061             :   // copy member variables
+    7062         104 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    7063             : 
+    7064             :   // heading of the point in drone frame
+    7065         104 :   double point_heading_horizontal = atan2(y, x);
+    7066             : 
+    7067         104 :   point_heading_horizontal += TAU;
+    7068             : 
+    7069             :   // if point_heading_horizontal is greater then 2*M_PI mod it
+    7070         104 :   if (fabs(point_heading_horizontal) >= TAU) {
+    7071         104 :     point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+    7072             :   }
+    7073             : 
+    7074             :   // heading of the right edge of the first sector
+    7075         104 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    7076             : 
+    7077             :   // calculate the idx
+    7078         104 :   int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+    7079             : 
+    7080         104 :   if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+    7081           0 :     idx -= bumper_data->n_horizontal_sectors;
+    7082             :   }
+    7083             : 
+    7084         208 :   return idx;
+    7085             : }
+    7086             : 
+    7087             : //}
+    7088             : 
+    7089             : // | ------------------------- safety ------------------------- |
+    7090             : 
+    7091             : /* //{ changeLandingState() */
+    7092             : 
+    7093          12 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+    7094             : 
+    7095             :   // copy member variables
+    7096          24 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7097             : 
+    7098             :   {
+    7099          12 :     std::scoped_lock lock(mutex_landing_state_machine_);
+    7100             : 
+    7101          12 :     previous_state_landing_ = current_state_landing_;
+    7102          12 :     current_state_landing_  = new_state;
+    7103             :   }
+    7104             : 
+    7105          12 :   switch (current_state_landing_) {
+    7106             : 
+    7107           4 :     case IDLE_STATE:
+    7108           4 :       break;
+    7109           8 :     case LANDING_STATE: {
+    7110             : 
+    7111           8 :       ROS_DEBUG("[ControlManager]: starting eland timer");
+    7112           8 :       timer_eland_.start();
+    7113           8 :       ROS_DEBUG("[ControlManager]: eland timer started");
+    7114           8 :       eland_triggered_ = true;
+    7115           8 :       bumper_enabled_  = false;
+    7116             : 
+    7117           8 :       landing_uav_mass_ = getMass();
+    7118             :     }
+    7119             : 
+    7120           8 :     break;
+    7121             :   }
+    7122             : 
+    7123          12 :   ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+    7124          12 : }
+    7125             : 
+    7126             : //}
+    7127             : 
+    7128             : /* hover() //{ */
+    7129             : 
+    7130           0 : std::tuple<bool, std::string> ControlManager::hover(void) {
+    7131           0 :   if (!is_initialized_)
+    7132           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7133             : 
+    7134           0 :   if (eland_triggered_)
+    7135           0 :     return std::tuple(false, "cannot hover, eland already triggered");
+    7136             : 
+    7137           0 :   if (failsafe_triggered_)
+    7138           0 :     return std::tuple(false, "cannot hover, failsafe already triggered");
+    7139             : 
+    7140             :   {
+    7141           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7142             : 
+    7143           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7144           0 :     std_srvs::TriggerRequest            request;
+    7145             : 
+    7146           0 :     response = tracker_list_.at(active_tracker_idx_)->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7147             : 
+    7148           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7149             : 
+    7150           0 :       return std::tuple(response->success, response->message);
+    7151             : 
+    7152             :     } else {
+    7153             : 
+    7154           0 :       std::stringstream ss;
+    7155           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'hover()' function!";
+    7156             : 
+    7157           0 :       return std::tuple(false, ss.str());
+    7158             :     }
+    7159             :   }
+    7160             : }
+    7161             : 
+    7162             : //}
+    7163             : 
+    7164             : /* //{ ehover() */
+    7165             : 
+    7166           4 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+    7167             : 
+    7168           4 :   if (!is_initialized_)
+    7169           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7170             : 
+    7171           4 :   if (eland_triggered_)
+    7172           0 :     return std::tuple(false, "cannot ehover, eland already triggered");
+    7173             : 
+    7174           4 :   if (failsafe_triggered_)
+    7175           0 :     return std::tuple(false, "cannot ehover, failsafe already triggered");
+    7176             : 
+    7177             :   // copy the member variables
+    7178           8 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7179           4 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7180             : 
+    7181           4 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7182             : 
+    7183           0 :     std::stringstream ss;
+    7184           0 :     ss << "can not trigger ehover while not flying";
+    7185           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7186             : 
+    7187           0 :     return std::tuple(false, ss.str());
+    7188             :   }
+    7189             : 
+    7190           4 :   ungripSrv();
+    7191             : 
+    7192             :   {
+    7193             : 
+    7194           4 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7195             : 
+    7196             :     // check if the tracker was successfully switched
+    7197             :     // this is vital, that is the core of the hover
+    7198           4 :     if (!success) {
+    7199             : 
+    7200           0 :       std::stringstream ss;
+    7201           0 :       ss << "error during switching to ehover tracker: '" << message << "'";
+    7202           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7203             : 
+    7204           0 :       return std::tuple(success, ss.str());
+    7205             :     }
+    7206             :   }
+    7207             : 
+    7208             :   {
+    7209           8 :     auto [success, message] = switchController(_eland_controller_name_);
+    7210             : 
+    7211             :     // check if the controller was successfully switched
+    7212             :     // this is not vital, we can continue without that
+    7213           4 :     if (!success) {
+    7214             : 
+    7215           0 :       std::stringstream ss;
+    7216           0 :       ss << "error during switching to ehover controller: '" << message << "'";
+    7217           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7218             :     }
+    7219             :   }
+    7220             : 
+    7221           4 :   std::stringstream ss;
+    7222           4 :   ss << "ehover activated";
+    7223           4 :   ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7224             : 
+    7225           4 :   callbacks_enabled_ = false;
+    7226             : 
+    7227           4 :   return std::tuple(true, ss.str());
+    7228             : }
+    7229             : 
+    7230             : //}
+    7231             : 
+    7232             : /* eland() //{ */
+    7233             : 
+    7234           8 : std::tuple<bool, std::string> ControlManager::eland(void) {
+    7235             : 
+    7236           8 :   if (!is_initialized_)
+    7237           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7238             : 
+    7239           8 :   if (eland_triggered_)
+    7240           0 :     return std::tuple(false, "cannot eland, eland already triggered");
+    7241             : 
+    7242           8 :   if (failsafe_triggered_)
+    7243           0 :     return std::tuple(false, "cannot eland, failsafe already triggered");
+    7244             : 
+    7245             :   // copy member variables
+    7246          16 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7247           8 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7248             : 
+    7249           8 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7250             : 
+    7251           0 :     std::stringstream ss;
+    7252           0 :     ss << "can not trigger eland while not flying";
+    7253           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7254             : 
+    7255           0 :     return std::tuple(false, ss.str());
+    7256             :   }
+    7257             : 
+    7258           8 :   if (_rc_emergency_handoff_) {
+    7259             : 
+    7260           0 :     toggleOutput(false);
+    7261             : 
+    7262           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7263             :   }
+    7264             : 
+    7265             :   {
+    7266           8 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7267             : 
+    7268             :     // check if the tracker was successfully switched
+    7269             :     // this is vital
+    7270           8 :     if (!success) {
+    7271             : 
+    7272           0 :       std::stringstream ss;
+    7273           0 :       ss << "error during switching to eland tracker: '" << message << "'";
+    7274           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7275             : 
+    7276           0 :       return std::tuple(success, ss.str());
+    7277             :     }
+    7278             :   }
+    7279             : 
+    7280             :   {
+    7281          16 :     auto [success, message] = switchController(_eland_controller_name_);
+    7282             : 
+    7283             :     // check if the controller was successfully switched
+    7284             :     // this is not vital, we can continue without it
+    7285           8 :     if (!success) {
+    7286             : 
+    7287           0 :       std::stringstream ss;
+    7288           0 :       ss << "error during switching to eland controller: '" << message << "'";
+    7289           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7290             :     }
+    7291             :   }
+    7292             : 
+    7293             :   // | ----------------- call the eland service ----------------- |
+    7294             : 
+    7295           8 :   std::stringstream ss;
+    7296             :   bool              success;
+    7297             : 
+    7298           8 :   if (elandSrv()) {
+    7299             : 
+    7300           8 :     changeLandingState(LANDING_STATE);
+    7301             : 
+    7302           8 :     odometryCallbacksSrv(false);
+    7303             : 
+    7304           8 :     ss << "eland activated";
+    7305           8 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7306             : 
+    7307           8 :     success = true;
+    7308             : 
+    7309           8 :     callbacks_enabled_ = false;
+    7310             : 
+    7311             :   } else {
+    7312             : 
+    7313           0 :     ss << "error during activation of eland";
+    7314           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7315             : 
+    7316           0 :     success = false;
+    7317             :   }
+    7318             : 
+    7319          16 :   return std::tuple(success, ss.str());
+    7320             : }
+    7321             : 
+    7322             : //}
+    7323             : 
+    7324             : /* failsafe() //{ */
+    7325             : 
+    7326           8 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+    7327             : 
+    7328             :   // copy member variables
+    7329          16 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7330           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7331           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7332             : 
+    7333           8 :   if (!is_initialized_) {
+    7334           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7335             :   }
+    7336             : 
+    7337           8 :   if (failsafe_triggered_) {
+    7338           0 :     return std::tuple(false, "cannot, failsafe already triggered");
+    7339             :   }
+    7340             : 
+    7341           8 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7342             : 
+    7343           0 :     std::stringstream ss;
+    7344           0 :     ss << "can not trigger failsafe while not flying";
+    7345           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7346           0 :     return std::tuple(false, ss.str());
+    7347             :   }
+    7348             : 
+    7349           8 :   if (_rc_emergency_handoff_) {
+    7350             : 
+    7351           0 :     toggleOutput(false);
+    7352             : 
+    7353           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7354             :   }
+    7355             : 
+    7356           8 :   if (getLowestOuput(_hw_api_inputs_) == POSITION) {
+    7357           0 :     return eland();
+    7358             :   }
+    7359             : 
+    7360           8 :   if (_parachute_enabled_) {
+    7361             : 
+    7362           0 :     auto [success, message] = deployParachute();
+    7363             : 
+    7364           0 :     if (success) {
+    7365             : 
+    7366           0 :       std::stringstream ss;
+    7367           0 :       ss << "failsafe activated (parachute): '" << message << "'";
+    7368           0 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7369             : 
+    7370           0 :       return std::tuple(true, ss.str());
+    7371             : 
+    7372             :     } else {
+    7373             : 
+    7374           0 :       std::stringstream ss;
+    7375           0 :       ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+    7376           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7377             :     }
+    7378             :   }
+    7379             : 
+    7380           8 :   if (_failsafe_controller_idx_ != active_controller_idx) {
+    7381             : 
+    7382             :     try {
+    7383             : 
+    7384          16 :       std::scoped_lock lock(mutex_controller_list_);
+    7385             : 
+    7386           8 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+    7387           8 :       controller_list_.at(_failsafe_controller_idx_)->activate(last_control_output);
+    7388             : 
+    7389             :       {
+    7390          16 :         std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7391             : 
+    7392             :         // update the time (used in failsafe)
+    7393           8 :         controller_tracker_switch_time_ = ros::Time::now();
+    7394             :       }
+    7395             : 
+    7396           8 :       failsafe_triggered_ = true;
+    7397           8 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    7398           8 :       timer_eland_.stop();
+    7399           8 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7400             : 
+    7401           8 :       landing_uav_mass_ = getMass();
+    7402             : 
+    7403           8 :       eland_triggered_ = false;
+    7404           8 :       ROS_DEBUG("[ControlManager]: starting failsafe timer");
+    7405           8 :       timer_failsafe_.start();
+    7406           8 :       ROS_DEBUG("[ControlManager]: failsafe timer started");
+    7407             : 
+    7408           8 :       bumper_enabled_ = false;
+    7409             : 
+    7410           8 :       odometryCallbacksSrv(false);
+    7411             : 
+    7412           8 :       callbacks_enabled_ = false;
+    7413             : 
+    7414           8 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+    7415             : 
+    7416             :       // super important, switch the active controller idx
+    7417             :       try {
+    7418           8 :         controller_list_.at(active_controller_idx_)->deactivate();
+    7419           8 :         active_controller_idx_ = _failsafe_controller_idx_;
+    7420             :       }
+    7421           0 :       catch (std::runtime_error& exrun) {
+    7422           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+    7423             :       }
+    7424             :     }
+    7425           0 :     catch (std::runtime_error& exrun) {
+    7426           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+    7427           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    7428             :     }
+    7429             :   }
+    7430             : 
+    7431          16 :   return std::tuple(true, "failsafe activated");
+    7432             : }
+    7433             : 
+    7434             : //}
+    7435             : 
+    7436             : /* escalatingFailsafe() //{ */
+    7437             : 
+    7438         149 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+    7439         298 :   std::stringstream ss;
+    7440             : 
+    7441         149 :   if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+    7442             : 
+    7443         141 :     ss << "too soon for escalating failsafe";
+    7444         141 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7445             : 
+    7446         141 :     return std::tuple(false, ss.str());
+    7447             :   }
+    7448             : 
+    7449           8 :   if (!output_enabled_) {
+    7450             : 
+    7451           0 :     ss << "not escalating failsafe, output is disabled";
+    7452           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7453             : 
+    7454           0 :     return std::tuple(false, ss.str());
+    7455             :   }
+    7456             : 
+    7457           8 :   ROS_WARN("[ControlManager]: escalating failsafe triggered");
+    7458             : 
+    7459           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7460           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7461             : 
+    7462          16 :   std::string active_tracker_name    = _tracker_names_.at(active_tracker_idx);
+    7463          16 :   std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    7464             : 
+    7465           8 :   EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+    7466             : 
+    7467           8 :   escalating_failsafe_time_ = ros::Time::now();
+    7468             : 
+    7469           8 :   switch (next_state) {
+    7470             : 
+    7471           0 :     case ESC_NONE_STATE: {
+    7472             : 
+    7473           0 :       ss << "escalating failsafe has run to impossible situation";
+    7474           0 :       ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7475             : 
+    7476           0 :       return std::tuple(false, "escalating failsafe has run to impossible situation");
+    7477             : 
+    7478             :       break;
+    7479             :     }
+    7480             : 
+    7481           2 :     case ESC_EHOVER_STATE: {
+    7482             : 
+    7483           2 :       ss << "escalating failsafe escalates to ehover";
+    7484           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7485             : 
+    7486           4 :       auto [success, message] = ehover();
+    7487             : 
+    7488           2 :       if (success) {
+    7489           2 :         state_escalating_failsafe_ = ESC_EHOVER_STATE;
+    7490             :       }
+    7491             : 
+    7492           2 :       return {success, message};
+    7493             : 
+    7494             :       break;
+    7495             :     }
+    7496             : 
+    7497           2 :     case ESC_ELAND_STATE: {
+    7498             : 
+    7499           2 :       ss << "escalating failsafe escalates to eland";
+    7500           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7501             : 
+    7502           4 :       auto [success, message] = eland();
+    7503             : 
+    7504           2 :       if (success) {
+    7505           2 :         state_escalating_failsafe_ = ESC_ELAND_STATE;
+    7506             :       }
+    7507             : 
+    7508           2 :       return {success, message};
+    7509             : 
+    7510             :       break;
+    7511             :     }
+    7512             : 
+    7513           2 :     case ESC_FAILSAFE_STATE: {
+    7514             : 
+    7515           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7516             : 
+    7517           2 :       ss << "escalating failsafe escalates to failsafe";
+    7518           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7519             : 
+    7520           4 :       auto [success, message] = failsafe();
+    7521             : 
+    7522           2 :       if (success) {
+    7523           2 :         state_escalating_failsafe_ = ESC_FINISHED_STATE;
+    7524             :       }
+    7525             : 
+    7526           2 :       return {success, message};
+    7527             : 
+    7528             :       break;
+    7529             :     }
+    7530             : 
+    7531           2 :     case ESC_FINISHED_STATE: {
+    7532             : 
+    7533           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7534             : 
+    7535           2 :       ss << "escalating failsafe has nothing more to do";
+    7536           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7537             : 
+    7538           4 :       return std::tuple(false, "escalating failsafe has nothing more to do");
+    7539             : 
+    7540             :       break;
+    7541             :     }
+    7542             : 
+    7543           0 :     default: {
+    7544             : 
+    7545           0 :       break;
+    7546             :     }
+    7547             :   }
+    7548             : 
+    7549           0 :   ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+    7550             : 
+    7551           0 :   return std::tuple(false, "escalating failsafe exception");
+    7552             : }
+    7553             : 
+    7554             : //}
+    7555             : 
+    7556             : /* getNextEscFailsafeState() //{ */
+    7557             : 
+    7558           8 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+    7559           8 :   EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+    7560             : 
+    7561           8 :   switch (current_state) {
+    7562             : 
+    7563           2 :     case ESC_FINISHED_STATE: {
+    7564             : 
+    7565           2 :       return ESC_FINISHED_STATE;
+    7566             : 
+    7567             :       break;
+    7568             :     }
+    7569             : 
+    7570           2 :     case ESC_NONE_STATE: {
+    7571             : 
+    7572           2 :       if (_escalating_failsafe_ehover_) {
+    7573           2 :         return ESC_EHOVER_STATE;
+    7574           0 :       } else if (_escalating_failsafe_eland_) {
+    7575           0 :         return ESC_ELAND_STATE;
+    7576           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7577           0 :         return ESC_FAILSAFE_STATE;
+    7578             :       } else {
+    7579           0 :         return ESC_FINISHED_STATE;
+    7580             :       }
+    7581             : 
+    7582             :       break;
+    7583             :     }
+    7584             : 
+    7585           2 :     case ESC_EHOVER_STATE: {
+    7586             : 
+    7587           2 :       if (_escalating_failsafe_eland_) {
+    7588           2 :         return ESC_ELAND_STATE;
+    7589           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7590           0 :         return ESC_FAILSAFE_STATE;
+    7591             :       } else {
+    7592           0 :         return ESC_FINISHED_STATE;
+    7593             :       }
+    7594             : 
+    7595             :       break;
+    7596             :     }
+    7597             : 
+    7598           2 :     case ESC_ELAND_STATE: {
+    7599             : 
+    7600           2 :       if (_escalating_failsafe_failsafe_) {
+    7601           2 :         return ESC_FAILSAFE_STATE;
+    7602             :       } else {
+    7603           0 :         return ESC_FINISHED_STATE;
+    7604             :       }
+    7605             : 
+    7606             :       break;
+    7607             :     }
+    7608             : 
+    7609           0 :     case ESC_FAILSAFE_STATE: {
+    7610             : 
+    7611           0 :       return ESC_FINISHED_STATE;
+    7612             : 
+    7613             :       break;
+    7614             :     }
+    7615             :   }
+    7616             : 
+    7617           0 :   ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+    7618             : 
+    7619           0 :   return ESC_NONE_STATE;
+    7620             : }
+    7621             : 
+    7622             : //}
+    7623             : 
+    7624             : // | ------------------- trajectory tracking ------------------ |
+    7625             : 
+    7626             : /* startTrajectoryTracking() //{ */
+    7627             : 
+    7628           2 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+    7629           2 :   if (!is_initialized_)
+    7630           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7631             : 
+    7632             :   {
+    7633           4 :     std::scoped_lock lock(mutex_tracker_list_);
+    7634             : 
+    7635           2 :     std_srvs::TriggerResponse::ConstPtr response;
+    7636           2 :     std_srvs::TriggerRequest            request;
+    7637             : 
+    7638             :     response =
+    7639           2 :         tracker_list_.at(active_tracker_idx_)->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7640             : 
+    7641           2 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7642             : 
+    7643           4 :       return std::tuple(response->success, response->message);
+    7644             : 
+    7645             :     } else {
+    7646             : 
+    7647           0 :       std::stringstream ss;
+    7648           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'startTrajectoryTracking()' function!";
+    7649             : 
+    7650           0 :       return std::tuple(false, ss.str());
+    7651             :     }
+    7652             :   }
+    7653             : }
+    7654             : 
+    7655             : //}
+    7656             : 
+    7657             : /* stopTrajectoryTracking() //{ */
+    7658             : 
+    7659           1 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+    7660           1 :   if (!is_initialized_)
+    7661           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7662             : 
+    7663             :   {
+    7664           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7665             : 
+    7666           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7667           1 :     std_srvs::TriggerRequest            request;
+    7668             : 
+    7669             :     response =
+    7670           1 :         tracker_list_.at(active_tracker_idx_)->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7671             : 
+    7672           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7673             : 
+    7674           2 :       return std::tuple(response->success, response->message);
+    7675             : 
+    7676             :     } else {
+    7677             : 
+    7678           0 :       std::stringstream ss;
+    7679           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'stopTrajectoryTracking()' function!";
+    7680             : 
+    7681           0 :       return std::tuple(false, ss.str());
+    7682             :     }
+    7683             :   }
+    7684             : }
+    7685             : 
+    7686             : //}
+    7687             : 
+    7688             : /* resumeTrajectoryTracking() //{ */
+    7689             : 
+    7690           1 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+    7691           1 :   if (!is_initialized_)
+    7692           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7693             : 
+    7694             :   {
+    7695           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7696             : 
+    7697           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7698           1 :     std_srvs::TriggerRequest            request;
+    7699             : 
+    7700           1 :     response = tracker_list_.at(active_tracker_idx_)
+    7701           1 :                    ->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7702             : 
+    7703           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7704             : 
+    7705           2 :       return std::tuple(response->success, response->message);
+    7706             : 
+    7707             :     } else {
+    7708             : 
+    7709           0 :       std::stringstream ss;
+    7710           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'resumeTrajectoryTracking()' function!";
+    7711             : 
+    7712           0 :       return std::tuple(false, ss.str());
+    7713             :     }
+    7714             :   }
+    7715             : }
+    7716             : 
+    7717             : //}
+    7718             : 
+    7719             : /* gotoTrajectoryStart() //{ */
+    7720             : 
+    7721           2 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+    7722           2 :   if (!is_initialized_)
+    7723           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7724             : 
+    7725             :   {
+    7726           4 :     std::scoped_lock lock(mutex_tracker_list_);
+    7727             : 
+    7728           2 :     std_srvs::TriggerResponse::ConstPtr response;
+    7729           2 :     std_srvs::TriggerRequest            request;
+    7730             : 
+    7731             :     response =
+    7732           2 :         tracker_list_.at(active_tracker_idx_)->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7733             : 
+    7734           2 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7735             : 
+    7736           4 :       return std::tuple(response->success, response->message);
+    7737             : 
+    7738             :     } else {
+    7739             : 
+    7740           0 :       std::stringstream ss;
+    7741           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'gotoTrajectoryStart()' function!";
+    7742             : 
+    7743           0 :       return std::tuple(false, ss.str());
+    7744             :     }
+    7745             :   }
+    7746             : }
+    7747             : 
+    7748             : //}
+    7749             : 
+    7750             : // | ----------------- service client wrappers ---------------- |
+    7751             : 
+    7752             : /* arming() //{ */
+    7753             : 
+    7754          18 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+    7755          36 :   std::stringstream ss;
+    7756             : 
+    7757          18 :   if (input) {
+    7758             : 
+    7759           0 :     ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+    7760           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7761           0 :     return std::tuple(false, ss.str());
+    7762             :   }
+    7763             : 
+    7764          18 :   if (!input && !isOffboard()) {
+    7765             : 
+    7766           0 :     ss << "can not disarm, not in OFFBOARD mode";
+    7767           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7768           0 :     return std::tuple(false, ss.str());
+    7769             :   }
+    7770             : 
+    7771          18 :   if (!input && _rc_emergency_handoff_) {
+    7772             : 
+    7773           0 :     toggleOutput(false);
+    7774             : 
+    7775           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7776             :   }
+    7777             : 
+    7778          18 :   std_srvs::SetBool srv_out;
+    7779             : 
+    7780          18 :   srv_out.request.data = input ? 1 : 0;  // arm or disarm?
+    7781             : 
+    7782          18 :   ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+    7783             : 
+    7784          18 :   if (sch_arming_.call(srv_out)) {
+    7785             : 
+    7786          18 :     if (srv_out.response.success) {
+    7787             : 
+    7788          18 :       ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+    7789          18 :       ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7790             : 
+    7791          18 :       if (!input) {
+    7792             : 
+    7793          18 :         toggleOutput(false);
+    7794             : 
+    7795          18 :         ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+    7796          18 :         timer_failsafe_.stop();
+    7797          18 :         ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+    7798             : 
+    7799          18 :         ROS_DEBUG("[ControlManager]: stopping the eland timer");
+    7800          18 :         timer_eland_.stop();
+    7801          18 :         ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7802             :       }
+    7803             : 
+    7804             :     } else {
+    7805           0 :       ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+    7806           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7807             :     }
+    7808             : 
+    7809             :   } else {
+    7810           0 :     ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+    7811           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7812             :   }
+    7813             : 
+    7814          36 :   return std::tuple(srv_out.response.success, ss.str());
+    7815             : }
+    7816             : 
+    7817             : //}
+    7818             : 
+    7819             : /* odometryCallbacksSrv() //{ */
+    7820             : 
+    7821          16 : void ControlManager::odometryCallbacksSrv(const bool input) {
+    7822          16 :   ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    7823             : 
+    7824          32 :   std_srvs::SetBool srv;
+    7825             : 
+    7826          16 :   srv.request.data = input;
+    7827             : 
+    7828          16 :   bool res = sch_set_odometry_callbacks_.call(srv);
+    7829             : 
+    7830          16 :   if (res) {
+    7831             : 
+    7832          15 :     if (!srv.response.success) {
+    7833           0 :       ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+    7834             :     }
+    7835             : 
+    7836             :   } else {
+    7837           1 :     ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+    7838             :   }
+    7839          16 : }
+    7840             : 
+    7841             : //}
+    7842             : 
+    7843             : /* elandSrv() //{ */
+    7844             : 
+    7845           8 : bool ControlManager::elandSrv(void) {
+    7846           8 :   ROS_INFO("[ControlManager]: calling for eland");
+    7847             : 
+    7848          16 :   std_srvs::Trigger srv;
+    7849             : 
+    7850           8 :   bool res = sch_eland_.call(srv);
+    7851             : 
+    7852           8 :   if (res) {
+    7853             : 
+    7854           8 :     if (!srv.response.success) {
+    7855           0 :       ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    7856             :     }
+    7857             : 
+    7858           8 :     return srv.response.success;
+    7859             : 
+    7860             :   } else {
+    7861             : 
+    7862           0 :     ROS_ERROR("[ControlManager]: service call for eland failed!");
+    7863             : 
+    7864           0 :     return false;
+    7865             :   }
+    7866             : }
+    7867             : 
+    7868             : //}
+    7869             : 
+    7870             : /* parachuteSrv() //{ */
+    7871             : 
+    7872           0 : bool ControlManager::parachuteSrv(void) {
+    7873           0 :   ROS_INFO("[ControlManager]: calling for parachute deployment");
+    7874             : 
+    7875           0 :   std_srvs::Trigger srv;
+    7876             : 
+    7877           0 :   bool res = sch_parachute_.call(srv);
+    7878             : 
+    7879           0 :   if (res) {
+    7880             : 
+    7881           0 :     if (!srv.response.success) {
+    7882           0 :       ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+    7883             :     }
+    7884             : 
+    7885           0 :     return srv.response.success;
+    7886             : 
+    7887             :   } else {
+    7888             : 
+    7889           0 :     ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+    7890             : 
+    7891           0 :     return false;
+    7892             :   }
+    7893             : }
+    7894             : 
+    7895             : //}
+    7896             : 
+    7897             : /* ungripSrv() //{ */
+    7898             : 
+    7899          80 : void ControlManager::ungripSrv(void) {
+    7900         160 :   std_srvs::Trigger srv;
+    7901             : 
+    7902          80 :   bool res = sch_ungrip_.call(srv);
+    7903             : 
+    7904          80 :   if (res) {
+    7905             : 
+    7906           0 :     if (!srv.response.success) {
+    7907           0 :       ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+    7908             :     }
+    7909             : 
+    7910             :   } else {
+    7911          80 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+    7912             :   }
+    7913          80 : }
+    7914             : 
+    7915             : //}
+    7916             : 
+    7917             : // | ------------------------ routines ------------------------ |
+    7918             : 
+    7919             : /* toggleOutput() //{ */
+    7920             : 
+    7921         116 : void ControlManager::toggleOutput(const bool& input) {
+    7922             : 
+    7923         116 :   if (input == output_enabled_) {
+    7924           6 :     ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+    7925           6 :     return;
+    7926             :   }
+    7927             : 
+    7928         110 :   ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+    7929             : 
+    7930         110 :   output_enabled_ = input;
+    7931             : 
+    7932             :   // if switching output off, switch to NullTracker
+    7933         110 :   if (!output_enabled_) {
+    7934             : 
+    7935          20 :     ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+    7936             : 
+    7937          20 :     switchTracker(_null_tracker_name_);
+    7938             : 
+    7939          20 :     ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+    7940             : 
+    7941          20 :     switchController(_eland_controller_name_);
+    7942             : 
+    7943             :     // | --------- deactivate all trackers and controllers -------- |
+    7944             : 
+    7945         140 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    7946             : 
+    7947         120 :       std::map<std::string, TrackerParams>::iterator it;
+    7948         120 :       it = trackers_.find(_tracker_names_.at(i));
+    7949             : 
+    7950             :       try {
+    7951         120 :         ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+    7952         120 :         tracker_list_.at(i)->deactivate();
+    7953             :       }
+    7954           0 :       catch (std::runtime_error& ex) {
+    7955           0 :         ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+    7956             :       }
+    7957             :     }
+    7958             : 
+    7959         120 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    7960             : 
+    7961         100 :       std::map<std::string, ControllerParams>::iterator it;
+    7962         100 :       it = controllers_.find(_controller_names_.at(i));
+    7963             : 
+    7964             :       try {
+    7965         100 :         ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+    7966         100 :         controller_list_.at(i)->deactivate();
+    7967             :       }
+    7968           0 :       catch (std::runtime_error& ex) {
+    7969           0 :         ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+    7970             :       }
+    7971             :     }
+    7972             : 
+    7973          20 :     timer_failsafe_.stop();
+    7974          20 :     timer_eland_.stop();
+    7975          20 :     timer_pirouette_.stop();
+    7976             : 
+    7977          20 :     offboard_mode_was_true_ = false;
+    7978             :   }
+    7979             : }
+    7980             : 
+    7981             : //}
+    7982             : 
+    7983             : /* switchTracker() //{ */
+    7984             : 
+    7985         216 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+    7986             : 
+    7987         648 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchTracker");
+    7988         648 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+    7989             : 
+    7990             :   // copy member variables
+    7991         432 :   auto last_tracker_cmd   = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7992         216 :   auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7993             : 
+    7994         432 :   std::stringstream ss;
+    7995             : 
+    7996         216 :   if (!got_uav_state_) {
+    7997             : 
+    7998           0 :     ss << "can not switch tracker, missing odometry!";
+    7999           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8000           0 :     return std::tuple(false, ss.str());
+    8001             :   }
+    8002             : 
+    8003         216 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8004             : 
+    8005           0 :     ss << "can not switch tracker, missing odometry innovation!";
+    8006           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8007           0 :     return std::tuple(false, ss.str());
+    8008             :   }
+    8009             : 
+    8010         216 :   if (rc_goto_active_) {
+    8011           0 :     ss << "can not switch tracker, the RC joystick is active";
+    8012           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    8013           0 :     return std::tuple(false, ss.str());
+    8014             :   }
+    8015             : 
+    8016         216 :   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+    8017             : 
+    8018             :   // check if the tracker exists
+    8019         216 :   if (!new_tracker_idx) {
+    8020           2 :     ss << "the tracker '" << tracker_name << "' does not exist!";
+    8021           2 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8022           2 :     return std::tuple(false, ss.str());
+    8023             :   }
+    8024             : 
+    8025             :   // check if the tracker is already active
+    8026         214 :   if (new_tracker_idx.value() == active_tracker_idx) {
+    8027          11 :     ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+    8028          11 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8029          11 :     return std::tuple(true, ss.str());
+    8030             :   }
+    8031             : 
+    8032             :   {
+    8033         203 :     std::scoped_lock lock(mutex_tracker_list_);
+    8034             : 
+    8035             :     try {
+    8036             : 
+    8037         203 :       ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_.at(new_tracker_idx.value()).c_str());
+    8038             : 
+    8039         203 :       auto [success, message] = tracker_list_.at(new_tracker_idx.value())->activate(last_tracker_cmd);
+    8040             : 
+    8041         203 :       if (!success) {
+    8042             : 
+    8043           0 :         ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+    8044           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8045           0 :         return std::tuple(false, ss.str());
+    8046             : 
+    8047             :       } else {
+    8048             : 
+    8049         203 :         ss << "the tracker '" << tracker_name << "' was activated";
+    8050         203 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8051             : 
+    8052             :         {
+    8053         406 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8054             : 
+    8055             :           // update the time (used in failsafe)
+    8056         203 :           controller_tracker_switch_time_ = ros::Time::now();
+    8057             :         }
+    8058             : 
+    8059             :         // super important, switch the active tracker idx
+    8060             :         try {
+    8061             : 
+    8062         203 :           ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+    8063         203 :           tracker_list_.at(active_tracker_idx_)->deactivate();
+    8064             : 
+    8065             :           // if switching from null tracker, re-activate the already active the controller
+    8066         203 :           if (active_tracker_idx_ == _null_tracker_idx_) {
+    8067             : 
+    8068          87 :             ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+    8069             :             {
+    8070         174 :               std::scoped_lock lock(mutex_controller_list_);
+    8071             : 
+    8072          87 :               initializeControlOutput();
+    8073             : 
+    8074         174 :               auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8075             : 
+    8076          87 :               controller_list_.at(active_controller_idx_)->activate(last_control_output);
+    8077             : 
+    8078             :               {
+    8079         174 :                 std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8080             : 
+    8081             :                 // update the time (used in failsafe)
+    8082          87 :                 controller_tracker_switch_time_ = ros::Time::now();
+    8083             :               }
+    8084             :             }
+    8085             : 
+    8086             :             // if switching to null tracker, deactivate the active controller
+    8087         116 :           } else if (new_tracker_idx == _null_tracker_idx_) {
+    8088             : 
+    8089          17 :             ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+    8090             :             {
+    8091          34 :               std::scoped_lock lock(mutex_controller_list_);
+    8092             : 
+    8093          17 :               controller_list_.at(active_controller_idx_)->deactivate();
+    8094             :             }
+    8095             : 
+    8096             :             {
+    8097          17 :               std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8098             : 
+    8099          17 :               last_tracker_cmd_ = {};
+    8100             :             }
+    8101             : 
+    8102          17 :             initializeControlOutput();
+    8103             :           }
+    8104             : 
+    8105         203 :           active_tracker_idx_ = new_tracker_idx.value();
+    8106             :         }
+    8107           0 :         catch (std::runtime_error& exrun) {
+    8108           0 :           ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+    8109             :         }
+    8110             :       }
+    8111             :     }
+    8112           0 :     catch (std::runtime_error& exrun) {
+    8113           0 :       ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+    8114           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8115             :     }
+    8116             :   }
+    8117             : 
+    8118         203 :   return std::tuple(true, ss.str());
+    8119             : }
+    8120             : 
+    8121             : //}
+    8122             : 
+    8123             : /* switchController() //{ */
+    8124             : 
+    8125         219 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+    8126             : 
+    8127         657 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchController");
+    8128         657 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+    8129             : 
+    8130             :   // copy member variables
+    8131         438 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8132         219 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8133             : 
+    8134         438 :   std::stringstream ss;
+    8135             : 
+    8136         219 :   if (!got_uav_state_) {
+    8137             : 
+    8138           0 :     ss << "can not switch controller, missing odometry!";
+    8139           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8140           0 :     return std::tuple(false, ss.str());
+    8141             :   }
+    8142             : 
+    8143         219 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8144             : 
+    8145           0 :     ss << "can not switch controller, missing odometry innovation!";
+    8146           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8147           0 :     return std::tuple(false, ss.str());
+    8148             :   }
+    8149             : 
+    8150         219 :   if (rc_goto_active_) {
+    8151           2 :     ss << "can not switch controller, the RC joystick is active";
+    8152           2 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    8153           2 :     return std::tuple(false, ss.str());
+    8154             :   }
+    8155             : 
+    8156         217 :   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+    8157             : 
+    8158             :   // check if the controller exists
+    8159         217 :   if (!new_controller_idx) {
+    8160           2 :     ss << "the controller '" << controller_name << "' does not exist!";
+    8161           2 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8162           2 :     return std::tuple(false, ss.str());
+    8163             :   }
+    8164             : 
+    8165             :   // check if the controller is not active
+    8166         215 :   if (new_controller_idx.value() == active_controller_idx) {
+    8167             : 
+    8168          34 :     ss << "not switching, the controller '" << controller_name << "' is already active!";
+    8169          34 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8170          34 :     return std::tuple(true, ss.str());
+    8171             :   }
+    8172             : 
+    8173             :   {
+    8174         181 :     std::scoped_lock lock(mutex_controller_list_);
+    8175             : 
+    8176             :     try {
+    8177             : 
+    8178         181 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_.at(new_controller_idx.value()).c_str());
+    8179         181 :       if (!controller_list_.at(new_controller_idx.value())->activate(last_control_output)) {
+    8180             : 
+    8181           0 :         ss << "the controller '" << controller_name << "' was not activated";
+    8182           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8183           0 :         return std::tuple(false, ss.str());
+    8184             : 
+    8185             :       } else {
+    8186             : 
+    8187         181 :         ss << "the controller '" << controller_name << "' was activated";
+    8188         181 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8189             : 
+    8190         181 :         ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_.at(new_controller_idx.value()).c_str(),
+    8191             :                  _tracker_names_.at(active_tracker_idx_).c_str());
+    8192             : 
+    8193             :         // reactivate the current tracker
+    8194             :         // TODO this is not the most elegant way to restart the tracker after a controller switch
+    8195             :         // but it serves the purpose
+    8196             :         {
+    8197         181 :           std::scoped_lock lock(mutex_tracker_list_);
+    8198             : 
+    8199         181 :           tracker_list_.at(active_tracker_idx_)->deactivate();
+    8200         181 :           tracker_list_.at(active_tracker_idx_)->activate({});
+    8201             :         }
+    8202             : 
+    8203             :         {
+    8204         362 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8205             : 
+    8206             :           // update the time (used in failsafe)
+    8207         181 :           controller_tracker_switch_time_ = ros::Time::now();
+    8208             :         }
+    8209             : 
+    8210             :         // super important, switch the active controller idx
+    8211             :         try {
+    8212             : 
+    8213         181 :           controller_list_.at(active_controller_idx_)->deactivate();
+    8214         181 :           active_controller_idx_ = new_controller_idx.value();
+    8215             :         }
+    8216           0 :         catch (std::runtime_error& exrun) {
+    8217           0 :           ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+    8218             :         }
+    8219             :       }
+    8220             :     }
+    8221           0 :     catch (std::runtime_error& exrun) {
+    8222           0 :       ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+    8223           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8224             :     }
+    8225             :   }
+    8226             : 
+    8227         181 :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+    8228             : 
+    8229             :   {
+    8230         181 :     std::scoped_lock lock(mutex_constraints_);
+    8231             : 
+    8232         181 :     sanitized_constraints_ = current_constraints_;
+    8233         181 :     sanitized_constraints  = sanitized_constraints_;
+    8234             :   }
+    8235             : 
+    8236         181 :   setConstraintsToControllers(sanitized_constraints);
+    8237             : 
+    8238         181 :   return std::tuple(true, ss.str());
+    8239             : }
+    8240             : 
+    8241             : //}
+    8242             : 
+    8243             : /* updateTrackers() //{ */
+    8244             : 
+    8245      127134 : void ControlManager::updateTrackers(void) {
+    8246             : 
+    8247      254268 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateTrackers");
+    8248      254268 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+    8249             : 
+    8250             :   // copy member variables
+    8251      127134 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8252      127134 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8253             : 
+    8254             :   // --------------------------------------------------------------
+    8255             :   // |                     Update the trackers                    |
+    8256             :   // --------------------------------------------------------------
+    8257             : 
+    8258           0 :   std::optional<mrs_msgs::TrackerCommand> tracker_command;
+    8259             : 
+    8260             :   unsigned int active_tracker_idx;
+    8261             : 
+    8262             :   {
+    8263      127134 :     std::scoped_lock lock(mutex_tracker_list_);
+    8264             : 
+    8265      127134 :     active_tracker_idx = active_tracker_idx_;
+    8266             : 
+    8267             :     // for each tracker
+    8268      891676 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    8269             : 
+    8270      764542 :       if (i == active_tracker_idx) {
+    8271             : 
+    8272             :         try {
+    8273             :           // active tracker => update and retrieve the command
+    8274      127134 :           tracker_command = tracker_list_.at(i)->update(uav_state, last_control_output);
+    8275             :         }
+    8276           0 :         catch (std::runtime_error& exrun) {
+    8277           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the active tracker (%s)",
+    8278             :                              _tracker_names_.at(active_tracker_idx).c_str());
+    8279           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8280           0 :           tracker_command = {};
+    8281             :         }
+    8282             : 
+    8283             :       } else {
+    8284             : 
+    8285             :         try {
+    8286             :           // nonactive tracker => just update without retrieving the command
+    8287      637408 :           tracker_list_.at(i)->update(uav_state, last_control_output);
+    8288             :         }
+    8289           0 :         catch (std::runtime_error& exrun) {
+    8290           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the tracker '%s'", _tracker_names_.at(i).c_str());
+    8291           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8292             :         }
+    8293             :       }
+    8294             :     }
+    8295             : 
+    8296      127134 :     if (active_tracker_idx == _null_tracker_idx_) {
+    8297       36579 :       return;
+    8298             :     }
+    8299             :   }
+    8300             : 
+    8301       90555 :   if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+    8302             : 
+    8303      181110 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8304             : 
+    8305       90555 :     last_tracker_cmd_ = tracker_command;
+    8306             : 
+    8307             :     // | --------- fill in the potentially missing header --------- |
+    8308             : 
+    8309       90555 :     if (last_tracker_cmd_->header.frame_id == "") {
+    8310           0 :       last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+    8311             :     }
+    8312             : 
+    8313       90555 :     if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+    8314           0 :       last_tracker_cmd_->header.stamp = ros::Time::now();
+    8315             :     }
+    8316             : 
+    8317             :   } else {
+    8318             : 
+    8319           0 :     if (active_tracker_idx == _ehover_tracker_idx_) {
+    8320             : 
+    8321           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the emergency tracker '%s' returned empty or invalid command!",
+    8322             :                          _tracker_names_.at(active_tracker_idx).c_str());
+    8323           0 :       failsafe();
+    8324             : 
+    8325             :     } else {
+    8326             : 
+    8327           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_.at(active_tracker_idx).c_str());
+    8328             : 
+    8329           0 :       if (_tracker_error_action_ == ELAND_STR) {
+    8330           0 :         eland();
+    8331           0 :       } else if (_tracker_error_action_ == EHOVER_STR) {
+    8332           0 :         ehover();
+    8333             :       } else {
+    8334           0 :         failsafe();
+    8335             :       }
+    8336             :     }
+    8337             :   }
+    8338             : }
+    8339             : 
+    8340             : //}
+    8341             : 
+    8342             : /* updateControllers() //{ */
+    8343             : 
+    8344      136855 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+    8345             : 
+    8346      273710 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateControllers");
+    8347      273710 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+    8348             : 
+    8349             :   // copy member variables
+    8350      136855 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8351             : 
+    8352             :   // | ----------------- update the controllers ----------------- |
+    8353             : 
+    8354             :   // the trackers are not running
+    8355      136855 :   if (!last_tracker_cmd) {
+    8356             : 
+    8357             :     {
+    8358       73160 :       std::scoped_lock lock(mutex_controller_list_);
+    8359             : 
+    8360             :       // nonactive controller => just update without retrieving the command
+    8361      219480 :       for (int i = 0; i < int(controller_list_.size()); i++) {
+    8362      182900 :         controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+    8363             :       }
+    8364             :     }
+    8365             : 
+    8366       36580 :     return;
+    8367             :   }
+    8368             : 
+    8369      200550 :   Controller::ControlOutput control_output;
+    8370             : 
+    8371             :   unsigned int active_controller_idx;
+    8372             : 
+    8373             :   {
+    8374      200550 :     std::scoped_lock lock(mutex_controller_list_);
+    8375             : 
+    8376      100275 :     active_controller_idx = active_controller_idx_;
+    8377             : 
+    8378             :     // for each controller
+    8379      601650 :     for (size_t i = 0; i < controller_list_.size(); i++) {
+    8380             : 
+    8381      501375 :       if (i == active_controller_idx) {
+    8382             : 
+    8383             :         try {
+    8384             :           // active controller => update and retrieve the command
+    8385      100275 :           control_output = controller_list_.at(active_controller_idx)->updateActive(uav_state, last_tracker_cmd.value());
+    8386             :         }
+    8387           0 :         catch (std::runtime_error& exrun) {
+    8388             : 
+    8389           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: an exception while updating the active controller (%s)",
+    8390             :                              _controller_names_.at(active_controller_idx).c_str());
+    8391           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8392             :         }
+    8393             : 
+    8394             :       } else {
+    8395             : 
+    8396             :         try {
+    8397             :           // nonactive controller => just update without retrieving the command
+    8398      401100 :           controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+    8399             :         }
+    8400           0 :         catch (std::runtime_error& exrun) {
+    8401             : 
+    8402           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_.at(i).c_str());
+    8403           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8404             :         }
+    8405             :       }
+    8406             :     }
+    8407             :   }
+    8408             : 
+    8409             :   // normally, the active controller returns a valid command
+    8410      100275 :   if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+    8411             : 
+    8412      100275 :     mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+    8413             : 
+    8414             :     // but it can return an empty command, due to some critical internal error
+    8415             :     // which means we should trigger the failsafe landing
+    8416             :   } else {
+    8417             : 
+    8418             :     // only if the controller is still active, trigger escalating failsafe
+    8419             :     // if not active, we don't care, we should not ask the controller for
+    8420             :     // the result anyway -> this could mean a race condition occured
+    8421             :     // like it once happend during landing
+    8422           0 :     bool controller_status = false;
+    8423             : 
+    8424             :     {
+    8425           0 :       std::scoped_lock lock(mutex_controller_list_);
+    8426             : 
+    8427           0 :       controller_status = controller_list_.at(active_controller_idx)->getStatus().active;
+    8428             :     }
+    8429             : 
+    8430           0 :     if (controller_status) {
+    8431             : 
+    8432           0 :       if (failsafe_triggered_) {
+    8433             : 
+    8434           0 :         ROS_ERROR("[ControlManager]: disabling control, the active controller returned an empty command when failsafe was active");
+    8435             : 
+    8436           0 :         toggleOutput(false);
+    8437             : 
+    8438           0 :       } else if (eland_triggered_) {
+    8439             : 
+    8440           0 :         ROS_ERROR("[ControlManager]: triggering failsafe, the active controller returned an empty command when eland was active");
+    8441             : 
+    8442           0 :         failsafe();
+    8443             : 
+    8444             :       } else {
+    8445             : 
+    8446           0 :         ROS_ERROR("[ControlManager]: triggering eland, the active controller returned an empty command");
+    8447             : 
+    8448           0 :         eland();
+    8449             :       }
+    8450             :     }
+    8451             :   }
+    8452             : }
+    8453             : 
+    8454             : //}
+    8455             : 
+    8456             : /* publish() //{ */
+    8457             : 
+    8458      136855 : void ControlManager::publish(void) {
+    8459             : 
+    8460      273710 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publish");
+    8461      273710 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+    8462             : 
+    8463             :   // copy member variables
+    8464      136855 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8465      136855 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8466      136855 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8467      136855 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8468      136855 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8469             : 
+    8470      136855 :   publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+    8471             : 
+    8472             :   // --------------------------------------------------------------
+    8473             :   // |                 Publish the control command                |
+    8474             :   // --------------------------------------------------------------
+    8475             : 
+    8476      136855 :   mrs_msgs::HwApiAttitudeCmd attitude_target;
+    8477      136855 :   attitude_target.stamp = ros::Time::now();
+    8478             : 
+    8479      136855 :   mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+    8480      136855 :   attitude_rate_target.stamp = ros::Time::now();
+    8481             : 
+    8482      136855 :   if (!output_enabled_) {
+    8483             : 
+    8484       22279 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+    8485             : 
+    8486      114576 :   } else if (active_tracker_idx == _null_tracker_idx_) {
+    8487             : 
+    8488       14302 :     ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+    8489             : 
+    8490             :     Controller::HwApiOutputVariant output =
+    8491       28604 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8492             : 
+    8493             :     {
+    8494       28604 :       std::scoped_lock lock(mutex_last_control_output_);
+    8495             : 
+    8496       14302 :       last_control_output_.control_output = output;
+    8497             :     }
+    8498             : 
+    8499       14302 :     control_output_publisher_.publish(output);
+    8500             : 
+    8501      100274 :   } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+    8502             : 
+    8503           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+    8504             :                       _controller_names_.at(active_controller_idx).c_str());
+    8505             : 
+    8506             :     Controller::HwApiOutputVariant output =
+    8507           0 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8508             : 
+    8509           0 :     control_output_publisher_.publish(output);
+    8510             : 
+    8511      100274 :   } else if (last_control_output.control_output) {
+    8512             : 
+    8513      100274 :     if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+    8514      100274 :       control_output_publisher_.publish(last_control_output.control_output.value());
+    8515             :     } else {
+    8516           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+    8517           0 :       return;
+    8518             :     }
+    8519             : 
+    8520             :   } else {
+    8521           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+    8522             :   }
+    8523             : 
+    8524             :   // | ----------- publish the controller diagnostics ----------- |
+    8525             : 
+    8526      136855 :   ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+    8527             : 
+    8528             :   // | --------- publish the applied throttle and thrust -------- |
+    8529             : 
+    8530      136855 :   auto throttle = extractThrottle(last_control_output);
+    8531             : 
+    8532      136855 :   if (throttle) {
+    8533             : 
+    8534             :     {
+    8535      109332 :       std_msgs::Float64 msg;
+    8536      109332 :       msg.data = throttle.value();
+    8537      109332 :       ph_throttle_.publish(msg);
+    8538             :     }
+    8539             : 
+    8540      109332 :     double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+    8541             : 
+    8542             :     {
+    8543      109332 :       std_msgs::Float64 msg;
+    8544      109332 :       msg.data = thrust;
+    8545      109332 :       ph_thrust_.publish(msg);
+    8546             :     }
+    8547             :   }
+    8548             : 
+    8549             :   // | ----------------- publish tracker command ---------------- |
+    8550             : 
+    8551      136855 :   if (last_tracker_cmd) {
+    8552      100274 :     ph_tracker_cmd_.publish(last_tracker_cmd.value());
+    8553             :   }
+    8554             : 
+    8555             :   // | --------------- publish the odometry input --------------- |
+    8556             : 
+    8557      136855 :   if (last_control_output.control_output) {
+    8558             : 
+    8559      231068 :     mrs_msgs::EstimatorInput msg;
+    8560             : 
+    8561      115534 :     msg.header.frame_id = _uav_name_ + "/fcu";
+    8562      115534 :     msg.header.stamp    = ros::Time::now();
+    8563             : 
+    8564      115534 :     if (last_control_output.desired_unbiased_acceleration) {
+    8565       86036 :       msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()(0);
+    8566       86036 :       msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()(1);
+    8567       86036 :       msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()(2);
+    8568             :     }
+    8569             : 
+    8570      115534 :     if (last_control_output.desired_heading_rate) {
+    8571       82774 :       msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+    8572             :     }
+    8573             : 
+    8574      115534 :     if (last_control_output.desired_unbiased_acceleration) {
+    8575       86036 :       ph_mrs_odom_input_.publish(msg);
+    8576             :     }
+    8577             :   }
+    8578             : }
+    8579             : 
+    8580             : //}
+    8581             : 
+    8582             : /* deployParachute() //{ */
+    8583             : 
+    8584           0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+    8585             :   // if not enabled, return false
+    8586           0 :   if (!_parachute_enabled_) {
+    8587             : 
+    8588           0 :     std::stringstream ss;
+    8589           0 :     ss << "can not deploy parachute, it is disabled";
+    8590           0 :     return std::tuple(false, ss.str());
+    8591             :   }
+    8592             : 
+    8593             :   // we can not disarm if the drone is not in offboard mode
+    8594             :   // this is super important!
+    8595           0 :   if (!isOffboard()) {
+    8596             : 
+    8597           0 :     std::stringstream ss;
+    8598           0 :     ss << "can not deploy parachute, not in offboard mode";
+    8599           0 :     return std::tuple(false, ss.str());
+    8600             :   }
+    8601             : 
+    8602             :   // call the parachute service
+    8603           0 :   bool succ = parachuteSrv();
+    8604             : 
+    8605             :   // if the deployment was successful,
+    8606           0 :   if (succ) {
+    8607             : 
+    8608           0 :     arming(false);
+    8609             : 
+    8610           0 :     std::stringstream ss;
+    8611           0 :     ss << "parachute deployed";
+    8612             : 
+    8613           0 :     return std::tuple(true, ss.str());
+    8614             : 
+    8615             :   } else {
+    8616             : 
+    8617           0 :     std::stringstream ss;
+    8618           0 :     ss << "error during deployment of parachute";
+    8619             : 
+    8620           0 :     return std::tuple(false, ss.str());
+    8621             :   }
+    8622             : }
+    8623             : 
+    8624             : //}
+    8625             : 
+    8626             : /* velocityReferenceToReference() //{ */
+    8627             : 
+    8628         720 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+    8629             : 
+    8630        1440 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8631        1440 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8632         720 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    8633             : 
+    8634         720 :   mrs_msgs::ReferenceStamped reference_out;
+    8635             : 
+    8636         720 :   reference_out.header = vel_reference.header;
+    8637             : 
+    8638         720 :   if (vel_reference.reference.use_heading) {
+    8639           0 :     reference_out.reference.heading = vel_reference.reference.heading;
+    8640         720 :   } else if (vel_reference.reference.use_heading_rate) {
+    8641         631 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+    8642             :   } else {
+    8643          89 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    8644             :   }
+    8645             : 
+    8646         720 :   if (vel_reference.reference.use_altitude) {
+    8647           0 :     reference_out.reference.position.z = vel_reference.reference.altitude;
+    8648             :   } else {
+    8649             : 
+    8650         720 :     double stopping_time_z = 0;
+    8651             : 
+    8652         720 :     if (vel_reference.reference.velocity.x >= 0) {
+    8653         498 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+    8654             :     } else {
+    8655         222 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+    8656             :     }
+    8657             : 
+    8658         720 :     reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+    8659             :   }
+    8660             : 
+    8661             :   {
+    8662         720 :     double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8663         720 :     double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8664             : 
+    8665         720 :     reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+    8666         720 :     reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+    8667             :   }
+    8668             : 
+    8669        1440 :   return reference_out;
+    8670             : }
+    8671             : 
+    8672             : //}
+    8673             : 
+    8674             : /* publishControlReferenceOdom() //{ */
+    8675             : 
+    8676      136855 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+    8677             :                                                  [[maybe_unused]] const Controller::ControlOutput&               control_output) {
+    8678      136855 :   if (!tracker_command || !control_output.control_output) {
+    8679       36581 :     return;
+    8680             :   }
+    8681             : 
+    8682      200548 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8683             : 
+    8684      200548 :   nav_msgs::Odometry msg;
+    8685             : 
+    8686      100274 :   msg.header = tracker_command->header;
+    8687             : 
+    8688      100274 :   if (tracker_command->use_position_horizontal) {
+    8689      100274 :     msg.pose.pose.position.x = tracker_command->position.x;
+    8690      100274 :     msg.pose.pose.position.y = tracker_command->position.y;
+    8691             :   } else {
+    8692           0 :     msg.pose.pose.position.x = uav_state.pose.position.x;
+    8693           0 :     msg.pose.pose.position.y = uav_state.pose.position.y;
+    8694             :   }
+    8695             : 
+    8696      100274 :   if (tracker_command->use_position_vertical) {
+    8697      100274 :     msg.pose.pose.position.z = tracker_command->position.z;
+    8698             :   } else {
+    8699           0 :     msg.pose.pose.position.z = uav_state.pose.position.z;
+    8700             :   }
+    8701             : 
+    8702             :   // transform the velocity in the reference to the child_frame
+    8703      100274 :   if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+    8704             : 
+    8705      100274 :     msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+    8706             : 
+    8707      200548 :     geometry_msgs::Vector3Stamped velocity;
+    8708      100274 :     velocity.header = tracker_command->header;
+    8709             : 
+    8710      100274 :     if (tracker_command->use_velocity_horizontal) {
+    8711      100274 :       velocity.vector.x = tracker_command->velocity.x;
+    8712      100274 :       velocity.vector.y = tracker_command->velocity.y;
+    8713             :     }
+    8714             : 
+    8715      100274 :     if (tracker_command->use_velocity_vertical) {
+    8716      100274 :       velocity.vector.z = tracker_command->velocity.z;
+    8717             :     }
+    8718             : 
+    8719      200548 :     auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+    8720             : 
+    8721      100274 :     if (res) {
+    8722      100274 :       msg.twist.twist.linear.x = res.value().vector.x;
+    8723      100274 :       msg.twist.twist.linear.y = res.value().vector.y;
+    8724      100274 :       msg.twist.twist.linear.z = res.value().vector.z;
+    8725             :     } else {
+    8726           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+    8727             :                          msg.child_frame_id.c_str());
+    8728             :     }
+    8729             :   }
+    8730             : 
+    8731             :   // fill in the orientation or heading
+    8732      100274 :   if (control_output.desired_orientation) {
+    8733       83984 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+    8734       16290 :   } else if (tracker_command->use_heading) {
+    8735       16290 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+    8736             :   }
+    8737             : 
+    8738             :   // fill in the attitude rate
+    8739      100274 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+    8740             : 
+    8741       78489 :     auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+    8742             : 
+    8743       78489 :     msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+    8744       78489 :     msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+    8745       78489 :     msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+    8746             :   }
+    8747             : 
+    8748      100274 :   ph_control_reference_odom_.publish(msg);
+    8749             : }
+    8750             : 
+    8751             : //}
+    8752             : 
+    8753             : /* initializeControlOutput() //{ */
+    8754             : 
+    8755         198 : void ControlManager::initializeControlOutput(void) {
+    8756             : 
+    8757         198 :   Controller::ControlOutput controller_output;
+    8758             : 
+    8759         198 :   controller_output.diagnostics.total_mass      = _uav_mass_;
+    8760         198 :   controller_output.diagnostics.mass_difference = 0.0;
+    8761             : 
+    8762         198 :   controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+    8763         198 :   controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+    8764             : 
+    8765         198 :   if (std::abs(_initial_body_disturbance_x_) > 0.001 || std::abs(_initial_body_disturbance_y_) > 0.001) {
+    8766           0 :     controller_output.diagnostics.disturbance_estimator = true;
+    8767             :   }
+    8768             : 
+    8769         198 :   controller_output.diagnostics.disturbance_wx_w = 0.0;
+    8770         198 :   controller_output.diagnostics.disturbance_wy_w = 0.0;
+    8771             : 
+    8772         198 :   controller_output.diagnostics.disturbance_bx_w = 0.0;
+    8773         198 :   controller_output.diagnostics.disturbance_by_w = 0.0;
+    8774             : 
+    8775         198 :   controller_output.diagnostics.controller = "none";
+    8776             : 
+    8777         198 :   mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+    8778         198 : }
+    8779             : 
+    8780             : //}
+    8781             : 
+    8782             : }  // namespace control_manager
+    8783             : 
+    8784             : }  // namespace mrs_uav_managers
+    8785             : 
+    8786             : #include <pluginlib/class_list_macros.h>
+    8787          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::control_manager::ControlManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..6b929840bc --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html @@ -0,0 +1,2217 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..529e9726528d313d5032fe6626aa29269edf3fcc GIT binary patch literal 26085 zcmV)VK(D`vP)oImN27<8*7l%>b^?=S<>EX~~zyb#;M%^k@kI_Pm z8qgq*YC!#R9X>{(_x7%3a+Ix^Jq8^-g$-? zN8rL3#|;x<95T>(e5}qjD`q2Aij1p$j1o?cn4va>jFIIdN09{BK29}82{0RH39xOP zEJmF%EHF|8Y-S{b(Ljt|hxce*4&w*Nqv3?au}1*xSlS4qg&4iv(>}OQ8soT)I$%8R zgFFtk0G89TEE?z4FVIr&c^2TQ4jTZMn*2wMHFGKgdf^e}DE;HHy!rr^GC2V7Sb_uW z86|LY1b3+19vsof`e{lHXzY3+C`G`LeNUJfDApyw;XR|Si`XZ0eRLA(*UcD=JH9&L zPP78!c&ZO-`BTV)t*J!J)ZFe7G~_V}Q@f4(F;l1p)No_oTn}?qe9p`~IzLwSxwsUR zfQuUAX&9ikLF;IccjgG}@F5qqV1p}LQU;7#QW1On-SY|kdBltF2#n0?>kj^F>7g_m#B+<|~x|KE}4@Yc~RZ#41Dwg%Q?(NpP2J zm+xtMOi{FJzkt^Cu3C!wHT+P*7y(DjC>mfb-b7_YVUpDt#~8V3Q}F)*ecGbhPqh|8 z6ybL)rW{bv>%r4m>UGI5mWWV=Q6j1XA2Ay{w#X_U!zNVaV_2yjER>>U5E3-}&P zr29KyjE>LrktaqMNVb1ZoZN7-8>rTXd?;@LEnWMi02z5qoak5z2`~GN3JY8fN*C8YWURDRaF z0-}(z_Zuhl*)|Xpw9*1ub4-`9VVn&|L`B!BBdxW!Lmn4kGaPr>JDzgD)8QEWbnZ8t zHedjkqa*g{Bu0U|3K`zjO+1dVq!0+VBhV6O#u^v><$eBE=oW^Mg?O6z$$p`u#du$6L*3$e#a0fRj` ztgIg_(vTR~>wiRkT&P3ai7B+Si##AZF*XB=GeZDNQ@EJ; z_&PxQ%q#_}A)swJaDKDS{TgB;BH#W!gN=7p&26_r`gS2yHlu*5s7GNggl@cKF>EuYzCz!#QZ}7;A!qyC#jeBWr+ai~wZ? z@D$-|fNkt?SCUPOt8c%K3p*ajp&z8l-xGb=$C6kwEab!eIIhCqG7pg(W5r5AgOe-( zma=vw6&)I3VxS{npbsGT*XS|AWmE{0!VB@{TNYP%IOs6zMYTW z>WR~FlucK}^+on*9*C?7E+is{og(Ilfv1U6@?Z~OPwssu#L%lS31i&?7XbUEhca9S zJkW*Teec@J#{w}&FuH??(Kbdm!eh905ssuSbA3GS;V}IH1E&!2Qy5}4M%AtTkRjqr zl9Jqj(Zy+00gA45ATbINQW1k&U{z()L?@FT%ig&l(q8HB&oaOiE*6;?hl|FTd!ziM zqevSFm`Gv)$x?7)#Cni&Q$1izW4Evet^XnE9O$M3-njT)>=@DdA2CQQh$tkG0m0$>B%NU;x-4^xhrJU^j;&M$j;R@Jcm*3rwA%< z37Z|bvWHWi>7M+jUO*1^D5yOFFnkXeY?0v=Km)^vYqMjvqX>XVFJx@$1zdM>{;33bf=A9WAVAtiOa+jhq`0A; z1gaTeftVap06b#QMP|=J<+;E|jCBAY8Rn?D-rEw?UJX!cNk#IOWIlkAAw;4vBIu?+FmH`=U#(m%8P-O~b%e+SK)|;8;yQvMMjrGyD#I9$C0w{i^%zGCGO#f6GL4q$ zIu5ol&|J&}J;edG5dnz-gd9bLkb7l}km1QE)-h5%#w8q1g$RxKq0nJAGtxL2hCe1H zpB!#xq*=66T|B|UnW=LgFogG4~Y$&Rt1u9=n{XHP;qVlY$Jjcy$o<q{;w19v!C#L^sf6y^pJ%#$NVR_74 zlQtjsjte;Hv8TPN2dN=9<(Q(W-M!rsf23XKs~R~%t+SeYUH#8=DrskNU<@Gs4Imul zu1|jO8O9|8TzuCCO34^?N)G{Q>+RLwV^WRrNTxdOi=Lf!j8fYV!(akLz&I8U*hEY? z=R&bo z3r&;sPy{1fcB$gATkL!|QnubY?)3u35@VWb=oL}SNB`(p7uDzdVZ;0dK_f=kN`P$`{Tpy1U>x`+_D;l%eKnEk8gfbm z+&o6j!*V)6k5Q6%@(yG^n_C1_HiS_SkG-`{;Yy`A{`^VnddQo*uI1~R*O48oaKpkN z0RH~F4p576({~+3EeC9)I!JELNl{6F;DrcePaKkN$%9hMBi~1;_--04@ASP>XCSr&ye!*c5J;h(jzbg=vZHJ8U!K;qGZJ6WcAdFyoN`PR-P4cLpqLVK8 zL)r`dAc66rW>3Pn$p_Nu7C;Gk12J2oh--!^JhUvd@)VLeMBXt>wFrpqM?HHy#?o2A z0qQ1OPxa5w{eHXCcD>zxaQg-PAu!?3Yxw!`zdHo{DYaif+KC9bk1-n)11OC#jxqxr zG)9A%p;_H_`PtGOC!u(zwTC+I(&6m+zV(FUSH(uZ*|tMI)o8M>x5xi)yF99<`gg8% zQ*D{+BEQpGS6L~W!0;U1Wym&+RJ3eAX22ytgj3h`CXW$e>lR$v*76w17&I7nly$&O zhcUTl^|qM~VYTas^10R?Iw-Yt{ky#a%VYo8=##?x^ok_$v57mc38SRQ&)3J!cKd84 zMc|r*;xQM%_#so%J~JEsIZjP=W^OmPjlAv5Acm{pn)D-{7`kNIX9iD9S~As{2@~^% zYbJM<6lMk;UDX?Vb>|wbnhm=`1b%wg&m1GsRPj3##-fsPqx}8qBa~Fib{P~BTb;td zERD&vX#l~RQ3H;VaMyHZp&!C9ssZ3gVm4*h7ws7J>~Vl&h*4Y3q2qB%3IYzBnN`*G zMSc1J2cP~iV)fVOeqhwI`Hp?0W09vCn7SqQ=r6iiK8k=Cblg&~sAk>yIt-v&%(5_+ zHw4TkTm~ffIwBywhU^E_k1+$(PGKxP*=#Bd=OYaz(HZPp3)O6W4TFI++%aTW*Y;fj zTz>Y>HJQTb>JfuEwPPG|5Z5tW>G~ucPq`%>X_4-u7~S=zG&I+dmXvlWfK@o4YvMQ` z#>Z&wX))4AH+*0Rw_K5fXLG&5t5R3=p3Md>X@2* zJjEb?7tvWDM#^ij8|;g^44Af7$|K8Y@(VAHhX;4 z&BqM!)^D3_M+$?E!ffggGJ8>*564tk;kmMaw*Y?QF;<)f#&BgqOCG&kAO_KLFB2nR z97K#ERYJR5bJ1SON)1avySN06OB8Nf@&(=hGC=N3*mJ0e*=f}qkh0lx@+kJMmCLn( zJ)SS@!8ilNN4>3pu??`17*$ekUF}8R-ta7qvoa}Hp_=19GjWWyeAmht6W~lfmIkTd z?|GoK<9j8wF%lun^w3I|LTih`o(6^a|y5+Ad>Hk7hL z8)4czX8OZnW>$HwRZUh>%^`zrN~$1{-AXEx$d8ama$Qh74XfHTE2UQY3hW zA%9YWag~wlSU?cXUB##e+|(FX6<|y{Ed!cE)6ctce~|VMm6G0I=62hpbyyY9{M{2h>9-~09lk>1pK^Q05|MEp9`>GJv{y< zt57N7;*|!uZ!}+NfDxvjNIsWzgVU9!_00t!?ngCX6EWXhfU#rz<^uGM@m0A1hA|eB z20-ed2P`JqhO42cCk=QsZ=E#Yzxnu$ohW;);0_3X0+)+Vh%J;upvQzi!4T+=5Q_dO zB;i2Qr*K|)XuL7}tnkq9D*X6Wm{p(eFOMnCHJa+@M)=|kk&l>}K@q-9W$N1_e0eQ7 zW^q9+`59B#65$(T-QXGjS@rgUPcP(j;<;8zk06MtWq|Mil*c=f+EXw^MKPzS;@DVR$@R|$B;L8 z^glUq?2<<)6!{qC$>=A>t-wS_4>8E8ux203b=b)R&!~84Zyimx@}rfHcxK#}oIQ}_ z5&U)$5Q;$saCFydz_S(*qAUX{F`kuOpS6INF#=e{%sB8m;J~i2ILm-U?3Kju?tQ*^ zAPcB$NCAMP{bRcVXxIxwe2p+hH8B8VG2^A@#)ro`VRz+JGu%H0lsJA!4ofwnQjWbK zDup*ZFd&$m^otze<}r#>NV~2AdW`bj#{~{6cilJGW?29mf6k9$R=O~7(TX<^X&67S zckCX93t5hb&q+ysoOh%^b&d3CrYkqgc+_zzlmXN090kCTN#c4gW89LmuT^VZ8!V|Z zP6I}yq~hiMaiEfN9Tm%~2cmRsk#Mp%s9`1y_mDMvNMe&&uO##cKgCn-)-wvHTDN7-H8$4CDwPYEybb zssZwE0ThUl6}fecag+r>EqlPO5ch*4@GP_zlv6k)0ei%Cux5Oe7R;XIi@)Jle}+*H zD6IZWoB09p7;Sg+Xe@xH^p5F_H&= z@X8u)Xx6iO5)a-x6kNETu!+V1gzb_*I@z2R`CKoqZAt1N?@qmSLDVq^v?IxT|>G#yb^tf$M59q9#4##;cR3f<9RtK55tD8hdu(mx5k%m4%z#P zQ>)&>U;q7|83S$NtsYo|A!AWFg_kG?MO6Tgd-6HTqyV_(%I$R6jRV59_oRUk8+!>+ z@e1jkso#zrmU(*taxe#rZv4owSC(jH? zPUDE#G{o?$=9$E7#ifAW=Aub{ntAyufI2akd_Zj1$|m?o89=oiu5MH8aBt-v2jOOH z+<3Qn0ZY5d?kTd=9rqL`xkDYq11inyZGw3n-tyh2r^N7Jx}L>e)5T)?N7)ClDKK6x zuGAmOVp0L_--xFmY=;XNoqJZ#$LKZ64v&QhOG~Q4M%;Wx-zvs1yqMd>x~z_YV+b2JS)k|G!HvfV&oYMJ_;j zpP9N`Pcat%@pk}t#)B5+6r%=tjJj_aM%12S*>?%<0_Vd%g}q4wjKQvFc<@!#0M!`r z89oww@;y7=a>vZ^F zpW2u=*C`hORU{5GZa94mFK=`R-jrdE$aB38l3C0cLsLkrCIZec2DE*wgDe0(#+L!K zRe$13d=u9vh@6R~ltK(mtTXV5g#%tBGkvMgq-$?nNZ%yLiK6aq5FN@67J=CE}sL&c*W;q-Z9DtG5ZD5_8=lt7_mN+ zi9Iw&gs!fQ0StM|85?tqtYN%O1JqN!@>&n>`MU~hfO@LGtFQ>D8RMT@g>%bU3%>o%n=|TNWB}!e zstjWQtaj}IxoZ~=kiFfT0H|s#AM;lpESYu`qjcWuy(QgoqZ#Lk*>T_*7ce?NdYd8w z!Wkcfu^F(LkH4(r0h=)<#Gsii`z6`-0|H6|#=4R!*c5R?Y_}<-rUl&Ex|T~3wU`Q+ zB`G5y0=g~kx_gm-ce&j)%4ZnyYa4*&$;rbaeR;r(9J_1RJ5LN`|b z%%031=#llG_tz~FKN2APHa3D$Nd!vQtzs~_w7ar}`~6ZJKeJ;{SK2IGt| z&YMD&gCutrT-+IQ-E-f4*7oD4!)FD>hPxT>-hAqnFC&R5+Q1?`8J0`0O1Ze8ggT%J z<36~pR4l%UYB_xE(iBheE5D`i2gL9Spt9>!;H-MI9{(J3?ilJL24m!-_A@{!4*&xC zUrvBr1_S}Dj8XDD=Gg@s3)tX__me%QhNWD&u!~;| zsA^a!3Q>7|Hx&Zl=+vD_ieic3UM0Y<5%)FXic|PC;#%AJFBx&_cK%v>quLB>Z!7yK zF8)PCWateE(YiqUv4n9vpk<8jEfnXkq4_m5DWIW^)=|P`0Yl7NB~{^4KcALVRV#32 z&O^$6o7*2cZUF-33{Wc+Zy&#U_>=;N3@&EfS}=;ICk*(1mrQiyR8&HAe3SrJP`7Z; z57!o9d~PK~P;IA}yo~lj@V!s~R-rAPQKVMT6a+F zk)w>Xnf93wpi!Km+~kto4=)QC$oYuEpdv|5kzQkl$enzOO|M8F!Dg;V!wsV#MIJ~= z2U(xLxTM1|3ikped})_(vZm8dBs*1CFR5j}4jpmTlFoQz=Kw(<5FHTf2iPo!T+i_M zM@Y>J4?;D)hqCy}2f?HYKP@0OA%rT#Sw4n@-1vuU$Vy@Cp0v1LshVF}gwe}_4)9zD zd;|6b$wrGZjjQp@Rd7Q#>|9h3$=6L@1-hgW{U$(j*F_)F1c!#e$83$y;iDPjOW9L! zK=e7U9LMp8S6Wi>Lc=9X6xb7Hg`Z+D*Y59gFwD>aZB1sc?Eo2^QNSzZ_A)qf6s*Xw zJqVP2zjmAE5;oQt^w+dsqjw;~?mgg8fm>EA$D@t$(EtD-07*naRN{&U98b&+bc3#Q zsbF~@0nZYkxk`+BEu57mfc+A=Dv{@8D+q1t={m1TL#bXbqBA4Sj5kxLGh@X(dhKx! zK4>AoXpsPmaaYEdPK#b{W7ZvH=C9QAfL_|4QaV<=s& z<>RjQ)=k)p8Zr8fy>?4lqa$^Fk&F}qsLNve@pW(cc8eLayVn){3z~Lc0QctHv#QCq z%yn|HbK<*h;f-!(LmN`Y%shEl$H$*LO+lu9XQTHz`mcFZ?#rtU;}0Ao$gsXEg{p4QFB52Xs_BeU%R_e-KuW7l`5`k<0@3FYU{c#uWDEAJYl@MyE<@RhnAdv3PabLF>3djSFXZi67W+d z;yn^R^ueFp6~|U}Q)6WA!}Ia}?IS6O7FaVtHAWO^pAd#Wc6+r6V;%5SCr$NI$i<9Z z6JIf_V-!6fTk?Rgo{tM**dC*4m_m{IY3tKL7!LtjR`s{88FmjRsP|=4w5h$u^hua0W(%O|R?z)h(m?>c*ak+JD+H zsvGFKKE~0dd8h3PL>}8E{O2o=KoHXz;C3hSc+buN2&1~it~FzPl{~(>veGHuX~y^T z!ZoA$#7>XROfKDOT{Bd4g2+TEx?0x^BU3D(&ed#?NApd^62ttKaOQKBEuVK&5uBJ* z?=@%SP=Sce7^?sy9(~K68u5{mj*~^Ip<@_W9`uuKj4{eHMQZX=v4p$*n;10{oGTgsvN#~Ll9Y{w$tzB7XP}wNe@P6;=|Ib`Nz}dmgjd9!neajbY zh>Isn*N1lm2od2}Hs;RMoil=%Ddg7V`qVefMj37}cU;#_g_WVsG!aRNSSEm*_(J%E z$W$2fRop%N&Q;17io|Hg=w>k48z-bU!m`K%2DZI+Htu}0q)Pn66H>7KZ%|vrkEVSF z>}iWndSC?PCvG$4+RlA!bxBGkjWBrc?s z5DHRVj>Oj#wgmVaOT0KD7-gM5&3nbCyX6a`6mSZ+%FN*%=`V{8pi8#jW_`zx;Vo&Wr&5#f$|v9iDpQ)3qy8RaS#GlF-~1GbYx_Vs;()<5aB}8FWNDNYB=#| z7=x*o&5UY{hhPGvWAaHb@;~5`bYgw8Q3cNOBPl3?bxDRz+mW=GN%Gng5>N7qWq$Z+ z`0@^?VYy(L)sL$y>14;K0W@HIsSkkh#Dy76R_>GA+{1k5RsM&zA3ivk*5`PW$o^AMr<~6JeEci2x8QlkvBoS&q9Xb6b z-3_B3H%q+~5-t!zAebpsHj42U{)+KtK87~De|@Ko%8Fhnu>HsfDs>UV4^RZ;h?!!M zhKSwZj$aloLP1C{&IG7EQaXgq=c+WRwr`WTq`E~V>QJ! z#wgtYU>n9^bFBfKG}p)KvgYrv^QmrL*V3xC#a@FGBna!voVob!6YdUq-D_RfoS3>i7*mMsegDJ; z=Jv$nlD(}J%x(bM7q710P=o#`0IFK0m1b_jrS6TB<1-$)K0X2=+ zX59%(ZcY78Nnu|!Vtg#GUy^Sw7JUH5#Bs2hlZQ7%K$_(vVa-VtTh#Y^cRkYN0kMzWvyH9H|;zEk(`QfxGKb<%W@KPxN*_4x8eAUu8 zGn$+3d}f#j^|sc^A%P~TaH$UCPi_q-HI)9lA(fIFyLh?s7()`|&7rp(0Y=}uoRL5Z z>*o^dhfe{#6v6;5Z3UCE3NpD+c$H9mKp8}^m0wz(6v6f-=Dmq2)04)yonbi7bBLK( z8)w+Ft@*YPHiWilMFwM6F=&HyEL?6(pFF^cFCMqw5~n;cSId9z7RwJ41J%ax4^=^P`B zyjVAv5#z$3TGosYBatHb&$-}AkD~vdg8P3*oWJvn+V7qOJ?Z^x5Tv=FzxI!Pb-Bt< z)oJ-SGkN!ogOw5=p)ZQ5Z}tv+EXHZSAs@4K(|FO1BhGKo_hP(lev>2?wL2D=Ezn__1Q@swGRO%Y>5 z*A+ku#@A&H6{>NjMlRSz4wwWthNTE^W!;QIk^Me@ai0M`zU1NtbBy;~RXh#wMwHLZ z$R4Fz=%7FE3GYtz2t^(_lXxNn@<*MrWnJ;CgsIu^_Qo<*%qwzQ6>Bs=T+}JK3eD1 zgd$^*49;uSKukP&il=pVVo#mCstSVGX{`QKEgY~J$4DG(>lSLTte{^T)d!%D%L@d( z&jU*RpJD~-#}B(t3WdpxsNaLxw(kF}KASsEMglC}UL(MI*ZMK4s}^O{gd#fhmb5?v z0({{eJ7T!RQ6`|Oz#bJb3FEjok;&p5fl=jbZoNc~bsd)McI6zR=L(TZR$|Uj*#P-% zCFXnYXrtOj}ctuhKg_-Q^&vB@PxQT#9 zjB#5p0}62;mR&(96lXr1qND3GU4$O@dcpq%jB5r=+4J9qNi>~?vvci}UNDc9q`>2z zP3LQHUugbyrg0ZycjCsr-fy?2OOpTeLdF`hc*P%5s&g%#B7S=DT^x>+iaw7Xy$3Lj2#ot3qjSil z00U=Q`Eq~eLH^wFqz%Zm2QwWW`{F(904}`>R>sKQgxkmH=K2_o&p1yZ^tEFth#CII zF=E|wBOBfo0;(}GB`;z;lUsuUP?ym8^P|(b9VyFUXgJ+1ZE=JxnY=wFXRs=zLzHd} zEjMSrs!M$F5_o(bALOp@q5SC;(v1NtiP{;)TTsjG#K0?VPg!;oxOa`Ssl_qsy8hv) zc|4EdX$sXPV6Htcu{p-_iZG55SrsL!<+-_KfU2%Xzb&nAK*B@yq9`8eQ1C;AVn{sz zK`2sLsoR?gMUiTb8e9kMV}wG75fY6RK9H1KpHT441o-w<5TT{Rpl)VUrJYqaC@f7>u#YcJLQ@Y^uED&_!+b%jb}M4pPP ze+~HY7iJG-CWBE?6mO!z01-c_p%qS$`J?C>LOe0b9U@MQD7vw0rGw4endRg9 zPqqq0nJ{#%ZY9$L_SLX`6eC?H2fHV?<1u)$mJDz~=LDWT8k4z=Jrx$UVa!$v82+D& z`W$@Y!$%&{yV%VVS@8Dhz>~_0u*<1!mVoxX?xD^xBn*vTZLeS3CAm?o=1t{+w)MP?8wFW`o_oS*>F{lQK~&74S!uN0Wq~$;<6>1Dq*2*xq95|EFCk9?1$1M;N=8nGkqhUx?p%uK zhqE$+&Qi7L8~sq}`rV@+GNBL-HA95r8~yNVq0nJGpM~<6P*SXWp`zxc-)FRduPTqJ zZ?DhifS2LnzHR@9iTNBab*84xEl8;(j~0ynfLR1=7-RY~cVC$t8afO59QrOjGjOq- z*gxS;v!YOXrm*;a$gI@H!FQU}l~SPE%AN{~+AwA-1q}bMzR^9iN(}wtd#7pWnh`^k z<@+JkL!A~n{&nM_$VZZxm){d$_i@62$~x3-k67!#c~PKG%vEiJJ!8rxPN07 zFDPaqk+h_Kq?eBAqqQbJlESAVpc*5e;|CD&x(crfRUEK`A#eajWH z@st59Uku3N-h>*ab@p%w=XU!hP8Wfg%r^yp@10o zSb9-#v3Ms*qJZ-kSAW`Ik&VRHY{uv^IW%1dHgb>tPOgFgGr77J*D)B5OE9={JnZ7S zUg^BrD((`L19C|@smSuB+c8SZi^*)_m)BDk7thJ=or_-XwPO^QS7vBR%WKZ!TB6%O zMdL1+IVT%&xDLdd8gaM)LnFqo5qBSP@OiEg_gQ{i=dJI@$mXWxU#?$2uCnWdcI&%= zbmmMvHhZsCIyOXeM*Ys2o){%c5QDV;EWHCugO$VwV+)~f{$)@AI64$~_)GARkF}Tl zU~Z$VvG|<}*!KYN<;B{3S~7;})ep0Dg=- zmIGZed+Mdj3+!oHu751X<~a1)mD#WJY^80^em(JZj^{ruo87x^y}6_r#yu@2j~!~u zPK~!$-AR5iy0`GQJPPHgSpZ7*;Fm$fP!UsjDBTI&78;M_uHqh&M|_f`d!fFK*3IWs zoBzz@AMU~#(zNoFMFCJdGif8P0jlEXEfJOamR3ON2K#*0y=Y+!IL(yqmQuz1KiV*& zhkr-BKXWd=Uf|VGGe*%A=on)dWEoI+Ghs~EqND;eSD|mBI2)J&WsUL~yS`Lf0IVtjJ$uv*jZi*lTKPCs-@EMwy06kMcJs%%{f%xN z7p+SHr0-@=%snkV=v4 zIf*{l-3Po&r=;^4xv%!WC1U8ju!j4G^? zI%$T&?hpU#$DB09q#a-nH0$vE+nUbrt#oX$WIpE)i}?!^2@fVVS&zxRRBy99#UjnNi=ku3_f)0w*YjR=Lgb z5JP;?^>moP7KY>NLcsE<6WqU;6Tj+5Qgz~&IwMC3cb>es;`yi7gAC_*!1@wG5U>EO zjgL?(t~81_u71B(99tbA^6M3D(JjUYI8c|z=xQg$AnNO#K96H0#&>uQnUg#@l9W{xy_h|M#r?>!I2VsGj1=ZXyOgbC@L< z7}xb}_a@y+W?ZA%E71L1wbvYH|1!0Ah>6zWEF}T%VmR80bfNw1O`M{lmxU$1(4HH0j5XGIR|W*G$2UYP|gcJYd# z$5fC24Hz{#>T4?0Fv5$RA&a4Kk%N4@A99fch$(oITXeX3=c2`eM2wWn__Q+PC^dYa zk#@LrnsgMIJj2(34csnpRO>NDT!R2DUH?Tl_n>`P);-PD@!muCl&+HCa#f7c3*@bA z4C~}PN{{aHQ;882 zi)RMfj?pZ*5C7s7^=a;U+L6?DE`;yMTz%Qp5P>micm0#w$C!)KKNcUJokQ1nd~2&P zpncjQ9{0EsJC|^Lv>*5OUmx{$Dy(f%dHqs{vEdCdc*JHa0ukQPVnmw&nDM>=P|HW9 zH`QeWhJsx#qiGFato}*S@IYT0S$Sq&{lYtX-F?eHUE(P&>URG`1T`+=?piL(H_5k0 zLTf=x1^gF|7)Q(X9f{GTUuvZ1tc(RlfJ3_mLKf!;DRLvmYPG%BI`?r(-iR><)$=RQ zX_fl&RV0wyKG)+O^Cqh>jTAn$+=g*b47<|z`RCrW^BJjwZVM`g<{IbzLk&$NuV9RN zVj{9(*4~7WK_BVjUx&U$&#JH1;6h+NHPR)-7Pmjy^14VDw=<60;{04gYv<}~PEq#Q zd;kL(K4%-wLlAoSXepYk#dzi_pYnj34LhAaph9L8nx(kG=&AV$hPao-t8l{*lLl%6 zSKi#Juwrklnp5yAn-kYGtHV&X~!n^6xHbe zyj;hY15hp3M{8TIl~*O=HX}Wc6PByRKzh*)q}`7z7us-qR5Khb8n#G$6Gj?BN}Cy@ z%DyHx#gBVz?4BQPGUG$K57cK>o{2hgqbD z@G?6r>%K4*3D_%5W^rS{E!=08h)G;$CCPZ@D^@^kszQ{Xx4c z-yYW{SbL!e`x1_OA&{m{wU~9D4_qu~t)yDl4&ZgCawt&wwJkEn*a~(?gP8dZjN7!# zV)iPAqh~VNx0q7bi42a@XFK3lci5l(aW8CLSNsBV+;ARWW2dy{2}*_c`lU|kzjUaG z-s5jFPzI=ue~c+74Dp_9bJwTvzGH0;l>zd!Sew-Jw%Ly>hNFo%Ps~uIK!GS}uU+ox z+k4kVcM>La+x0oO_9_XV5J(P0yWo*)%$B~&ynN-Mf-l0D?{)LC0YhPxm>5XwHGYoZ z;`X`WCO9i*NQ!7;O9ik2<2`Om%updQKaUhBi5!e^OmPDR4+&tIL;r79 z4{b(VP3^sIi;E=I3{7p_Nu1FG&i=}`$!qurwh9J9s6ml)j}M)F~fp}I4{ z7L!1$R(5uqrd&cBSzK!6B9Vk8d@yikLk^a699g>Vmue1;K579{ek~V;ZM% z`y^Kpj<@~+!EJtJ`75|XS}wS~rfyAeKP|VJ7u;-&dSbqUJEDybfUdnj7urW0&eVas zS#`YR3P2+_QhgOYkMXo?_aN2oe4@)PrkV)9fB;n&5Q;JC+T6->rI6vz2;B9NE_JvM zsT}g;XEKA_{|`zkcZH5qQbGAyl~l}fs{y9SE9n;?pl)UqW7M_TO6qnGmC!QEze=jo z^K?t9KNb0FNu_0-R(ivOx;0DcX6B!@9raaG)t*0~*l%4)-Fz8lWqXGQ z#|K8bBrXgJe+tDzZ`6Pzy5-gP*Dt(`|Eo8U&pG()cI8JcSD}o3560ok^$uP*=T_`@ zxt0MxZMlBJODu03Gdwu)%O>H;@d^5+fR?WRq=b(V^SOuIx`YFV??;#0`VsheBOh=a zDX3!MBQaw8FRGW-fkMq|!? z2*cGIlCtX`ZVnZIKUAklfVob)O$5|a&B_`4^6D|delQ@AGnCnMb&LIazvqHx&6Rrl z%%(e3W;bE9HmmvX+PKqWxkCGvlr1;@jV$oSi~sPoadi*NJtTxJgXDt*)9Y?><=f3B zxX-GDn8Qky5dH5#KrJyJQVEgSDgU~SJELx)G|wvmL?!a^7}oIJcG=+DSL-Ev!Y{|D zc@XU~_+2Rj!<8G7uH(E;oK)a613XimCIRL;O?=*Q0HB^~R?gs;4}tsIxEclE*Opsd z0>dLI}#;5ongIpA8srdHdz~08UcMs-FFneNQ6>QE#2u4I+ zd?7|AmM=bKVfcO{eZ3Lcxsq9EXLtdy4WsDiCV;@towJ7s#TEQk=NQEjkj7Q`qx76x z{bs6)l4^|E>dMaycMMoGD-5QBdd(>P-2MK?JMnYBm}=Fo**@2jvpEC&5uDAr^a!ru zIe>bqHO}T8BkHt(Q8n2Iy)``1Gkgzmo5H=q{ZBc3k0Bn`S!hfAw0~>w@Y8Yu{--dB zd!Qk%Vve)0glEtm0-&<-_pasYp)n6iD2@g2-O{eh02;;^9SBr~He-yq1_4^S{zFQN zJayQOQBotl)aX;(Fz}uH2G-pqLGiwGo$}7*O5**IQEG%Fo7Jz$HoaehPqE zk0$e2t?z&bEIfxE01aJBwQ*1n-x4F`_UYBg;MC8mjWcxTdYk21c#QEw#jI1%zF|8> zKy`bqgr{BMa!KkT%|p|g+2YuwX|w`f615dT80Equh3h);j|Eh=!`VeG{O)lpo{{`* zKxV$uv;x2>ZzBKYO>?Fl4tCA&rdgv}5NQhd8Di$840u3~!4ul)`W#VR;L(bLyC%{7 zZo}pFHfu)SyO_oG+jAWw9AX(GCi=O68)0JtH1JV8& zdD#|ZU3GH zDJNfRFE>+Co9J|5j1M;Y+()yua5l_hH0k1Al;K5}A| z?Yn)H0oCoVynS``N%>laf42{Iz3vCaGm8KKAOJ~3K~(L#eZ2n^dZBkC=DH@#{gc}V z{!_tM~=^`72J7?}YhtoT#_ z{BhO!ul|4cdn$}|gDWg6902O=pxXOUr=sSC>e{J;XpNlUpUcy+4k zN-Mj+>2m$niK_tWsn*wtW5wrsyN7by4Oj*Nl>tdOi;g2l- zfPMIy-hxBx_wjXcU5EYS@!^&0`a_;h1dhTnp>9yBcvEw=ErsxF-=h>OkJ={NX81UT zk5b_z#d^6uz&7jMKA1FrwU|MU7YxH>&IB>L-Te=Dbb$R^=FEp2a*s<;j$y3etZUCD z6RSz=A5-Asu!8M@=#GjPlH#3kVuvf?A<;K#1w5*R-fsE1aD>J!Uu8tV$GXNRX^HCL z>A=Y8Qj|Tr+5@t}#^VOS4KBhGAh!r-11cGQ78cD)NVh8cx`S2* zwP4v5bnT!%0-S{$dYbb;1yHvPCv91%*}dRieb+^a_^_J_7l5dMgfG4=6j`~ zz=$hK54h97QneA|B>k|j>+Bd4BsmkE7 zrC`>0JN(Rw9}~7CTf-Xx)C+Dd|IE7RJ99-Z;fesWz7s(k0@S*i3=9{Q&$=e{W3_Pu zwRAo(U;{?Z4xb8WA7k#$MJ87ZM%1z!p9w-yYnzol^!EXm3Dr#)R~B6pMp2jajj?dy z-KVcyVrEU;`lh>LM%N1RSA0DP?8 zjEuf`-Mijn^xYJ?sQvv;#r^G+zq=7nM!+L{P?T_;;0Cf&x5xMqjnB+xzTCkD-W`HBXZa}^%LoCZu#_Tc74Ctp+nFEDMVL2dO z!vU@Q_yS<2Ir%6%D1h7+;w{S83*$53ef3bUd## ztPw*Xt-lcSVF3hd6;_5|!ky749_Dyf@ylBk}L9%XQW*8aqZI zw1}Rsh0Z}UcCFB7HjEnn6ajOL`mRCzToF)_)foUiZ??py)qMZLvk!U8ckD|e z%v10}%88OYjQ>$k74dLZJ9^Fcn{9z|x-d+^#fa_=|C9hfNX&-5Xv5StWCX*;aF2+6 zhrI_Qp35>I5SsR?Zm&O*g4;Z~Yd#r>7?6eY3~m!Vaq7jTX3x}uAcNKyTXUu_*j<~3 zulD#rH->PYP;UUaDDZ#Q><+s)uz41`CB~Ma!E^?cMP>) z+=t34e=XpY%Xlz_VFsg{`!!R@l^AJbxac76ah&r369&L}Q@DUIuM_yQ;iUm7mmHNr zO1A*%pYc=%Pl(0Pe3?aRXSlDo6|RQDq8Z5W++c?smL0k}KB@uVhT(zjrxW~Q#9JB{ zUfF+=4`AF$g+HcTP`0=i3MG5q;Ruj58hD|e>zDfgqym4aYqbwR+x4D$<+qsd0#C|{ zUQ*Z6Jr4IohB!NDuK#oQgI9aKg$u-WP5$A#uFGw_=?d0$*8Kb=TeNn!BnL zX9vyof4#1UIqkW1%^g4f({;^Z?J*kwPeyPm(#^oyl9gZVzg3u>sC9m~m>s4=|_ z=(>~k#mywR+$zk!JNP0x3XC(HfNLOlP}QQu$Ku;|y*~S{`aS8$xD*ZN(o3gKAmJ1; zt>fvNbLgNv>C@f@OyOwq#bTK~(IP1YoFcR3!`QQwC8#Dnf%alNE|j1TWse|1>cv$l zy%4kDEXlj!7t5EM6SE6aeQ#lPRi(#^Mp1g;>iR_K31L$W?C}eyCOxsd!AD3>pt|rn z2AINHNW7cEIyQ(=0jL#<56IF2#_*`q8S8ZE%hNbi!*ByrH8 z8fAX8-}4K&G0I&--0P=!*nQm9D2$g(Dtc>&J?A{*vCzw=K#rVF?f{LtC;Vao@K5TV zVU8hG3=CvZ-c#X-n{h7;2ySRIbyaO{wBV)^BN^Q-ma({lEKoykGl*r_J(?nav{UG9 zid(ckQ-!WaVZ>Na>jAj4;PE3V*3ev`D!u?$10;rzubL3b)7!dv{wzZm;TQ&^rsFone#tW9BBi0?avC^P>3r_hhE-nH`F zV>tNC*j=HYJC=iQ4faV66$np|~^)11=0;o9_OlPM}bZ6ILL z-jNaV+7zgoW61IEoyJJ#F`WNiol2$<@Q(@l`syATP~E<|r@|hk?g>*^o1jUF4S#iA z4_MkWg1$asT?;!9S(~rkkeJEH@|z|_u(4(I+9Q0Yl+mL{DXbaYXHyN|(Pu98T&GeJ z@cDIyJ>##~dWC{Ib-zL}S}4jgF>%u@x0Rb~?l$ZE-CbGkHLV%#?)sy2&wRwr^$Gj+ zcXur_{0$W8cX#Chl^Fk~P<(e+u6<~jL|QYjLcWqav1Y*$1aqeM6OXOAq$va^U)~L4 z9A8=uZ`hl(7}_%9vrHroaR0LMs%;ZsY<zt?x`K(}% zRxAkFABc5wK=)H3P~k%ox{PH_fa#Kja>t7U}cE_M?YvG@G(=Z zrqhSI#j{Dg)8WSm7+!eoFdw4;s|Rc+MtlVdNsmko!*Cu}52$WuD&YYodF_Q+3g98b zxZpSIDvZeFbMY&WDYTZ|X(2y0?Qz(ND$*%CWkCK@dni>vf_qqUH3z_|Y? z+oU8}GMDaHV#LxlJ*KdMZ#m#u0yK1;-IH=8v1Xx|r>X2ZyvD&+291T~I=11Je;GOW zNb9fAPw|nK1DnA+XY?OfTMGE30)4+^^Xz%0lJc25OQ-_IKRA#Z0JwU%eQ|+DvZ5PO z*w`=7+=6fJB)#Sy26>cKC;v!VdY;EJM)0&LKLx3-0v3V{qq=J(Whm}qyTk?h`v=5` zsH9CV695sK>E#e=v zG=^Bwf&=6}PkW6O@ll+@A|Ku3c^j*mGr)IdIQn4@!;^xKIk!h|Qif*=IfhFiT53Md zxe&wm00S_3dpP10kM?KJ79d&zpsMSz8fanuY_mb5yC9XEl?69Y4X{9_!ih447IBq% zGI8DkHA4O%e6Zzn&dj~;C)UoH^uVWUi_*h7y!xewd2W^P-yY)O0-pL;XEw_MrBl z`d%p{5!Q@2g>;Ogo%?FF2gZol)O8DxZo*FBWg;dN$-lt@6FZhu_$r(u93Xh0Y5{1E z`HrZiTJmKAZp;q^%FZ95Y*GN!_|4`<49SIotwI3>;dlz=>KD1|OSGssdCbQMTI(2- z=SqB}lZ8 ze6IO3lKoimXKdyzj^}yP<0WJ-jIC#jn{eFII2F34kaa3>Q^;$pb_W{`>{$Vz8l$99 zGC=LlHSff}(*DteDCeHPP6uE0P!%3n|0q~E-hlkexa@pS4 z3(T;&?yJLkkuxE7#?o~-B!uIt*ma)Slow{VMUf;K-v(SuN0Tc|_!3X+IOvvfaq&woB!`9%O!UL(s3zd|C=cr1k`nX-jy{&0*T7mi>v0r z?zwR_E!bTVx&Lv&?N=f&!*M>?c}cp;13A7+B|s2Oh1xFB~&8iu8wegE^07w(}$ zP1iU^o4;Pm0naqY1p-bFiIE(UHmX;V1dOo?L@-urD;mZ~w)XXsVyQ&HLf{9_a-6bueP!}0aWhx(f5;!Dsrg+1CiN4(wqeHjJK4YMdf1}gg(6PP zt+Z-(!0YWEUpioTy?@=zo+~Fg9&q#{2Or`hIgT>bU4vFk7VzEVVTYF_MZodOzPao8 zA6B?+T^Fqtxofi1w7gzaIV0&$NboL-2eig2z@Fj#PzI>QsMJwThu=c(ec01)NZCd} zK`q~IP!kz9q`ESjSR_I*Gg^kvDKfSOVd7j#(PgI|qe}(|fjdj|l;o;7!~sQzop;;i zV_aEDZRX>Tb$5-{;4RsOx3K4?_qpnl+is;uqWjY4qHJ|E3u1N&uMIYdSqdfNqIbNQ z9l_1niOKbOPceIZy!mf#7BPF7P$Vx!#R53HZKz!4>rC%HpoQeKu3(;~Zf2CB*Ns`e zMeodKxVcCBDUi)VY}~fol#jJ>yBr#QHg2CoWBe_^U$${$t*KiMs8QjcV&iJAAKDmR zu9Dx#o?FWFyrlx!cW1=Cy88U(wRU)YjPGS2xf6RbkG^(zt-Ylko{#bU#0;PJKEe)% z!3y3xm5Mbt1uGaV`(T>{ffA)h-St}8VXQ*I1T_xEIAn}#sZ-_BjJZrk3nH^9? zD}93HPRf=EfZK(fxde>HY!_Intpun)h|fKy6tbq7qZD&yoxJ4HYKF>w}^Y7muaoi*8dO-E3hjlh~m}fCcyLDZk&rE>mqsBw{ z!S};wOEb-slBeU&StdJ-+DP@I#q|ASsExsl26i1~0rwia9$aAn_FOtgHb!%o#>iom zqPbPg5_@gOSUnB1?C~E&KvmZ{!yD@$#bEbJvZNhjnVdF^C62d_5mXKcO2~gQXZgm5 z)xuj|`$w|Abh`O`uIN2THKaz$Th?oeebc<5E%V0hj>Si^yg9t@#uMrRD2GTbDKf-l zI?xCW@&HY&^kaG(a7u&*idES7HTFUNp3^|54_?viCCTcR0$B1z_9)p%d?|Ua@gL1D zGk4gqMiMpj#TZrrzRb%-4ge{F_vVdu|G`pCLO1{rFTg>04mTXcr~1tXCHr8t zd|}&q=1;@f5!<+2L65~Z)Qs*0I>LHiYoO8XFJ0okcI z%$e>fT}!Lp=lM^k5PF77lHQ)C)sJxkd!A?G-b2i?yP$5faf^VTVdKJj)*<;RJv!7> zu62^tT)1uCmC-8`-g~RC_S{P4;VZ0zp8&cqISp-!e>|x|Q$ts^X1lfDxtPfV-j>== zA7qjmFupD5$;l{8f7i8;A0%_G%cvYOjLvtM+7! z$l4pB_pk^GYVTL?ebrvX#-W^`_T29mp9LfsrEFZ^zG|=Be!gmNNSmwn##DjBBFL(} zUDUs7FJj|ReyzP4#;@9|VEn2*86&Khlpc2qU$%M+N0uU~)QWc*uTH{~A{m;SqPWsf zW5fcUYEIlkkxYNDX342g3HZF41yPrYd}?5i_vD94B$a18K7u{DmTzHCY{fC^NO^Tt zj<{Tz?YxCOV6%2_3$E+7ZoV!+FGfGmJs58Fr=}3_XL(zi?A0S4$|gqHfCte+5aR0I zZ(IYZYf!?Emn4}A14Duk*8&ku#He!UTQNE|arx;Kuw0gh#YjS%`^QL&4A|I{VPr}S zOo&2OTckfRSL$&(a#>HV@{NC<>E=GnsBSWvmII*0?P*t7$mekmt-9%R7^F`;e;0V+;>@-H41-Hv10XA-h9pc!NN{Qw(A{z2mQ@cErl8bCEhyt~Ra zE>$VM;C-sIiUD)1yOi(HG)z*HhpQ=(~l_<|1 z&-TQ*w z$8GG455PO&urwQ&LMnDEx=OK7tf2S!3ZZi=I6%dOPbphQIO7MnGimc4XCr$8UkXOJ z3F9GDn?0SvD+aDYW%g|A@EUAft=~BH!TY)zLl81~96H7*Y-`jzo3C%yf$_pkrYYvb z>5AwJIHi}HjxD<4cZ|Dut{W+9w&Jy zZY{3LZ`riC>K&i_)4LWEimUcZ&jsgDj*uQ44>qY5IlgG#Ik0*DtC-;$l z5PNFU!%%$|=PJ|uW92?NqOqzH~p_oT;x3+l&kiVJu1;fS-#@D>c34FG21yk(Iy z0T7p9dsh8$%$^VpTxJhivy4L{vM1J%3k!SQq7UN29w3>jj)Xl7z)beME>&XcqeOs} z^bBV^u`}f{nP!g!tc3zvb_FiIz`dpo>RDfzBLKfIWR+|r4QZ;tGWdq(d=}LdDq_l? zxtMimIv47e%^CJxIGWnJR=pR6x(pR79 zW8|7qC^X6<{D#2sFsoL1JSda}?K<>Dl|=xoijSITYm2K3F0LgVycbW*1`k^N`@5qT zS3JeN&EldB04m04Y>K%qQ{6r@$*m#zH6~DvdnWUib*;I9v~|s_QaT3x#_p`W`rB#F z+QZf(dDqSPl6OGO$yq5n2LoK=GjM?v@c!F9Jpz*fqW9n8JK}Hj44mYY{;Yh3QPB!m z>2QZQ_b-3e*Zez9;c9G8@hfb*Dc(f74FSaJ?T_3pm~% z{=F_JHvYQc@sR>fzAoV9A_HVzI5M)wDbjwYOZe6MpK!W_-~Je zM+a>@Uzb`G8fY;eW9CQt4ge}KYFmC#0Lnkxxm2n--I2Ry9a7%jq5v!q2|mPMDHOXE zUN-0hG5Z<+;TM78xOP=xUFA@?CSoi`TnZdXGER>oJKn%%*|&drf^CL!Xm2sW;!2hR zSO3-7G)0Wi1VVtfLiJgbS*UKqv+b=y;Sa6LDM<*D4JZ_hEIe8g+*b;PcCKG!*{d7! zUGXs&k>RX9%owiBfHGLkdSckq_m+KV*%idVhsYi)#-#KFKqW?yOS+&O(r_Ga$tBGh z`S8I}s(%>wBd)U&O8w0!Z$!tK5|PVeyxs8Vq5^nXxde4XMIzvXr$nV$E0T-4EEJW% z^-|=W5MZp=OVN&TY6*CT>TA3&webj9XfV=OZ9|awl>+`~8E$&jwu*8_ z(&^u-XCKc-gAy=#2lI+QZI-*?PdeIPgrma6Z|!w;Q;jw|@neSXb zm-(4d6Qf-WXXR=oe^LMd1y4ytK~%-7>0FrX`WWJSOkV_6oRuX8_}e`=$$q>KKy6{? zDGBdyP@pE>kaArsTkgi`*MqN%13+$WC}+(EJ8 z?Y83$lUN9Ym~~>;7#}~;p2_UJP!=%O&;U)ca)}#PwD+LYx z{JWKk-DOL|vV-yNc`3OX>{aP_r z0JRvgi$tKin`&<+B(fVYDtswAjECXcr$9AGBrra7sP}Ryss4aHbCL&XU=`-vgVcRD ztn(m2z|Tmk+Jxcl1_;j55CGQ;4L2#KX~6)NLIg|0%y4J|ji$M7iF8S36tpYmMY>?B zS464A9xiAW0Jbt*3do~lTv;e%*C1)A2&l$buGgGZ7*JqDDgJV?rV(R|`1h0z(^&zx zqu>pGKwp@)f)PC0Q~kJ1GP#nEY@Qb9H`8A4tC#EW>dLa1A!nK`tCHA8$sh#xg(zZ( zJ4-~{SI-``JW73ni1WP}U%uc*&lqt!jAUlgnrWYFA^D0%eXtVFwSy}}q7sUd3-A^m zt-X4{x|#dR5QK_5GbRyF$nuSMami3X*Kzi{IK@mG#SGSKPqs`dV$(E6sWx3})TgRI z-Rz6StyZsjU+<#9LjW%oxM>^GS}GeDHHEHC=3}wzGv;Fn@MUp(->m__KxOd1l38G^qprAdsgwzy9uLTU%U(h!1H;~;jEOWO|tj#zX`uK1#p5k z;|(M}!nZO&cWL*mF}k4?X67On@EWbnKF3?=G|Cu{AwKgk*Ce+P;Twu?d4I8FB_7Z{ z1)pykjA5mr3GQ)bK5c#KM+D9&?3*Vov)NS0w{ zCcaCmWDm`+r@z$5o*|3tv-U!7PIqTlW2;0mS1ftM=(0(^Yr2fgZSK263gzlJzy^%P zcix-sJ^*e8u#gKBnv-zJQAzRU2S~wda8P;`O&FEzQ8zx8i@76iW z2)2qm=sXub;%*s?X-b6Go%{uLd15~F9& z>-Y$WE|+Wls4^(SPOybd+VQNGHMC}cAfA;jR;VUloFK9?LKp~nv@k|x*Pt>+ktQof zoFvhUQRU>yJep*sHUVlkcD!kFdD75eR{_qfxLac5YuPi!2LN2~{xBX7=SdFkpl{R$j(Z`H6R9Iq-byaX_z}-mocX3t4T$?mvoCN z!|AFuTXpza;h`O)#K&zi;B)Ffau`4Fm@?v;O+2QM1=i@YgE{jVU=v1w55)nCfKJ{< zc74guHwHj`*X+i+Qe`LwO78myybGQ4FtJ262_sVj4w#vjbj<{k^b + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2422371365.2 %
Date:2024-04-25 05:42:57Functions:9612378.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
64.8%64.8%
+ 		64.8 %2379 / 367075.7 %84 / 111
<unnamed>64.8 %2379 / 367075.7 %84 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
<unnamed>100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-detail-sort-l.html b/mrs_uav_managers/src/control_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..5c1d6e09a7 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2422371365.2 %
Date:2024-04-25 05:42:57Functions:9612378.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
64.8%64.8%
+ 		64.8 %2379 / 367075.7 %84 / 111
<unnamed>64.8 %2379 / 367075.7 %84 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
<unnamed>100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-detail.html b/mrs_uav_managers/src/control_manager/index-detail.html new file mode 100644 index 0000000000..8b957c17b9 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2422371365.2 %
Date:2024-04-25 05:42:57Functions:9612378.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
64.8%64.8%
+ 		64.8 %2379 / 367075.7 %84 / 111
<unnamed>64.8 %2379 / 367075.7 %84 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
<unnamed>100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-sort-f.html b/mrs_uav_managers/src/control_manager/index-sort-f.html new file mode 100644 index 0000000000..f2554c0649 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2422371365.2 %
Date:2024-04-25 05:42:57Functions:9612378.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
64.8%64.8%
+ 		64.8 %2379 / 367075.7 %84 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-sort-l.html b/mrs_uav_managers/src/control_manager/index-sort-l.html new file mode 100644 index 0000000000..19b7a9b73a --- /dev/null +++ b/mrs_uav_managers/src/control_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2422371365.2 %
Date:2024-04-25 05:42:57Functions:9612378.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
64.8%64.8%
+ 		64.8 %2379 / 367075.7 %84 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index.html b/mrs_uav_managers/src/control_manager/index.html new file mode 100644 index 0000000000..081adc4046 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2422371365.2 %
Date:2024-04-25 05:42:57Functions:9612378.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
64.8%64.8%
+ 		64.8 %2379 / 367075.7 %84 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html new file mode 100644 index 0000000000..d35f12b19e --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-04-25 05:42:57Functions:1212100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)94
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()94
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1314
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
mrs_uav_managers::control_manager::OutputPublisher::publish(std::variant<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >, mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >, mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >, mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >, mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >, mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >, mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >, mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >, mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)114576
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html new file mode 100644 index 0000000000..a43263de7c --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-04-25 05:42:57Functions:1212100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3776
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8028
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)494
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1314
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)92790
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3779
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1062
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2330
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1003
mrs_uav_managers::control_manager::OutputPublisher::publish(std::variant<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >, mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >, mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >, mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >, mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >, mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >, mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >, mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >, mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)114576
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)94
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()94
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.frameset.html b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.frameset.html new file mode 100644 index 0000000000..43585be242 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html new file mode 100644 index 0000000000..4b893a5a51 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html @@ -0,0 +1,154 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-04-25 05:42:57Functions:1212100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <control_manager/output_publisher.h>
+       2             : 
+       3             : namespace mrs_uav_managers
+       4             : {
+       5             : 
+       6             : namespace control_manager
+       7             : {
+       8             : 
+       9          94 : OutputPublisher::OutputPublisher() {
+      10          94 : }
+      11             : 
+      12          94 : OutputPublisher::OutputPublisher(ros::NodeHandle& nh) {
+      13             : 
+      14          94 :   ph_hw_api_actuator_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>(nh, "hw_api_actuator_cmd_out", 1);
+      15          94 :   ph_hw_api_control_group_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>(nh, "hw_api_control_group_cmd_out", 1);
+      16          94 :   ph_hw_api_attitude_rate_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>(nh, "hw_api_attitude_rate_cmd_out", 1);
+      17          94 :   ph_hw_api_attitude_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>(nh, "hw_api_attitude_cmd_out", 1);
+      18          94 :   ph_hw_api_acceleration_hdg_rate_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd>(nh, "hw_api_acceleration_hdg_rate_cmd_out", 1);
+      19          94 :   ph_hw_api_acceleration_hdg_cmd_      = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>(nh, "hw_api_acceleration_hdg_cmd_out", 1);
+      20          94 :   ph_hw_api_velocity_hdg_rate_cmd_     = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>(nh, "hw_api_velocity_hdg_rate_cmd_out", 1);
+      21          94 :   ph_hw_api_velocity_hdg_cmd_          = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>(nh, "hw_api_velocity_hdg_cmd_out", 1);
+      22          94 :   ph_hw_api_position_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>(nh, "hw_api_position_cmd_out", 1);
+      23          94 : }
+      24             : 
+      25      114576 : void OutputPublisher::publish(const Controller::HwApiOutputVariant& control_output) {
+      26             : 
+      27      114576 :   std::visit(OutputPublisher::PublisherVisitor(this), control_output);
+      28      114576 : }
+      29             : 
+      30             : // | ------------------------- private ------------------------ |
+      31             : 
+      32        3776 : void OutputPublisher::publish(const mrs_msgs::HwApiActuatorCmd& msg) {
+      33        3776 :   ph_hw_api_actuator_cmd_.publish(msg);
+      34        3776 : }
+      35             : 
+      36        3779 : void OutputPublisher::publish(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      37        3779 :   ph_hw_api_control_group_cmd_.publish(msg);
+      38        3779 : }
+      39             : 
+      40       92790 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      41       92790 :   ph_hw_api_attitude_rate_cmd_.publish(msg);
+      42       92790 : }
+      43             : 
+      44        8028 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      45        8028 :   ph_hw_api_attitude_cmd_.publish(msg);
+      46        8028 : }
+      47             : 
+      48        1003 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      49        1003 :   ph_hw_api_acceleration_hdg_rate_cmd_.publish(msg);
+      50        1003 : }
+      51             : 
+      52        1062 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      53        1062 :   ph_hw_api_acceleration_hdg_cmd_.publish(msg);
+      54        1062 : }
+      55             : 
+      56        2330 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      57        2330 :   ph_hw_api_velocity_hdg_rate_cmd_.publish(msg);
+      58        2330 : }
+      59             : 
+      60        1314 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      61        1314 :   ph_hw_api_velocity_hdg_cmd_.publish(msg);
+      62        1314 : }
+      63             : 
+      64         494 : void OutputPublisher::publish(const mrs_msgs::HwApiPositionCmd& msg) {
+      65         494 :   ph_hw_api_position_cmd_.publish(msg);
+      66         494 : }
+      67             : 
+      68             : }  // namespace control_manager
+      69             : 
+      70             : }  // namespace mrs_uav_managers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.overview.html b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.overview.html new file mode 100644 index 0000000000..8bbe8c2bfd --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.overview.html @@ -0,0 +1,38 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.png b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c385c662de6cc4327e3a61ce14e679273fb7fbb9 GIT binary patch literal 307 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1=!VDx=Zs3yuQW60^A+G=b|6c_J%iqVwzWUF= zunH&+rp|e9UJ7J$7I;J!GcfQS0b$0e+I-SL!GoSIjv*eMZ^In<8Web%cPzx|uYg9wc$$GsO#c<%LD z^o61EWbIvRy=0gZWQ-p596VxsjnPP{OElF*Cg6-{dy|A>Y}=H;oU<(ImnK^5lUTi3 z+iSA*={Nb$&wWaG6~8WP+rHV`1hgbCr~Q%KEqQ|=`dh-AP literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..f5fff4463f --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37353769.5 %
Date:2024-04-25 05:42:57Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::EstimationManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::switchToHealthyEstimator()0
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()5
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)5
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)5
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)94
mrs_uav_managers::estimation_manager::EstimationManager::onInit()94
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()94
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)128
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)304
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const304
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const608
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const3162
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const17984
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)18974
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)176475
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)176475
mrs_uav_managers::estimation_manager::StateMachine::isInSwitchableState() const176475
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const195444
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const371827
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1693085
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html new file mode 100644 index 0000000000..738b75627e --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37353769.5 %
Date:2024-04-25 05:42:57Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)304
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()5
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)176475
mrs_uav_managers::estimation_manager::EstimationManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)176475
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)5
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)94
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)5
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)18974
mrs_uav_managers::estimation_manager::EstimationManager::switchToHealthyEstimator()0
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)128
mrs_uav_managers::estimation_manager::EstimationManager::onInit()94
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()94
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const304
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const371827
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const608
mrs_uav_managers::estimation_manager::StateMachine::isInSwitchableState() const176475
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const195444
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const17984
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1693085
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const3162
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..0e76ed7e74 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html new file mode 100644 index 0000000000..4f7f4a35f9 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html @@ -0,0 +1,1338 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37353769.5 %
Date:2024-04-25 05:42:57Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*//{ includes */
+       2             : #include <ros/ros.h>
+       3             : #include <nodelet/nodelet.h>
+       4             : 
+       5             : #include <pluginlib/class_loader.h>
+       6             : #include <nodelet/loader.h>
+       7             : 
+       8             : #include <geometry_msgs/QuaternionStamped.h>
+       9             : #include <geometry_msgs/Vector3Stamped.h>
+      10             : 
+      11             : #include <nav_msgs/Odometry.h>
+      12             : 
+      13             : #include <std_srvs/Trigger.h>
+      14             : #include <std_srvs/SetBool.h>
+      15             : 
+      16             : #include <mrs_msgs/UavState.h>
+      17             : #include <mrs_msgs/Float64Stamped.h>
+      18             : #include <mrs_msgs/String.h>
+      19             : #include <mrs_msgs/EstimationDiagnostics.h>
+      20             : #include <mrs_msgs/HwApiCapabilities.h>
+      21             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      22             : 
+      23             : #include <mrs_lib/param_loader.h>
+      24             : #include <mrs_lib/publisher_handler.h>
+      25             : #include <mrs_lib/service_client_handler.h>
+      26             : #include <mrs_lib/transformer.h>
+      27             : #include <mrs_lib/subscribe_handler.h>
+      28             : #include <mrs_lib/gps_conversions.h>
+      29             : #include <mrs_lib/scope_timer.h>
+      30             : 
+      31             : 
+      32             : #include <mrs_uav_managers/state_estimator.h>
+      33             : #include <mrs_uav_managers/agl_estimator.h>
+      34             : #include <mrs_uav_managers/estimation_manager/support.h>
+      35             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      36             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      37             : /*//}*/
+      38             : 
+      39             : namespace mrs_uav_managers
+      40             : {
+      41             : 
+      42             : namespace estimation_manager
+      43             : {
+      44             : 
+      45             : // --------------------------------------------------------------
+      46             : // |                           classes                          |
+      47             : // --------------------------------------------------------------
+      48             : 
+      49             : /*//{ class StateMachine */
+      50             : class StateMachine {
+      51             : 
+      52             :   /*//{ states */
+      53             : public:
+      54             :   typedef enum
+      55             :   {
+      56             : 
+      57             :     UNINITIALIZED_STATE,
+      58             :     INITIALIZED_STATE,
+      59             :     READY_FOR_FLIGHT_STATE,
+      60             :     TAKING_OFF_STATE,
+      61             :     FLYING_STATE,
+      62             :     HOVER_STATE,
+      63             :     LANDING_STATE,
+      64             :     LANDED_STATE,
+      65             :     ESTIMATOR_SWITCHING_STATE,
+      66             :     DUMMY_STATE,
+      67             :     EMERGENCY_STATE,
+      68             :     FAILSAFE_STATE,
+      69             :     ERROR_STATE
+      70             : 
+      71             :   } SMState_t;
+      72             : 
+      73             :   /*//}*/
+      74             : 
+      75             : public:
+      76        1316 :   StateMachine(const std::string& nodelet_name) : nodelet_name_(nodelet_name) {
+      77          94 :   }
+      78             : 
+      79     1693085 :   bool isInState(const SMState_t& state) const {
+      80     1693085 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) == state;
+      81             :   }
+      82             : 
+      83      371827 :   bool isInitialized() const {
+      84      371827 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) != UNINITIALIZED_STATE;
+      85             :   }
+      86             : 
+      87      195444 :   bool isInPublishableState() const {
+      88      195444 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      89      150174 :     return current_state == READY_FOR_FLIGHT_STATE || current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE ||
+      90      345623 :            current_state == LANDING_STATE || current_state == DUMMY_STATE || current_state == FAILSAFE_STATE;
+      91             :   }
+      92             : 
+      93      176475 :   bool isInSwitchableState() const {
+      94      176475 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      95      176475 :     return current_state == READY_FOR_FLIGHT_STATE || current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE ||
+      96      176475 :            current_state == LANDING_STATE;
+      97             :   }
+      98             : 
+      99             :   bool isInTheAir() const {
+     100             :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+     101             :     return current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE || current_state == LANDING_STATE;
+     102             :   }
+     103             : 
+     104             :   SMState_t getCurrentState() const {
+     105             :     return mrs_lib::get_mutexed(mtx_state_, current_state_);
+     106             :   }
+     107             : 
+     108       17984 :   std::string getCurrentStateString() const {
+     109       17984 :     return mrs_lib::get_mutexed(mtx_state_, sm_state_names_[current_state_]);
+     110             :   }
+     111             : 
+     112         608 :   std::string getStateAsString(const SMState_t& state) const {
+     113         608 :     return sm_state_names_[state];
+     114             :   }
+     115             : 
+     116             :   /*//{ changeState() */
+     117         304 :   bool changeState(const SMState_t& target_state) {
+     118             : 
+     119         304 :     if (target_state == current_state_) {
+     120             : 
+     121           0 :       ROS_WARN("[%s]: requested change to same state %s -> %s", getPrintName().c_str(), getStateAsString(current_state_).c_str(),
+     122             :                getStateAsString(target_state).c_str());
+     123           0 :       return true;
+     124             :     }
+     125             : 
+     126         304 :     switch (target_state) {
+     127             : 
+     128           0 :       case UNINITIALIZED_STATE: {
+     129           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is not possible from any state", getPrintName().c_str(), getStateAsString(UNINITIALIZED_STATE).c_str());
+     130           0 :         return false;
+     131             :         break;
+     132             :       }
+     133             : 
+     134          94 :       case INITIALIZED_STATE: {
+     135          94 :         if (current_state_ != UNINITIALIZED_STATE) {
+     136           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(INITIALIZED_STATE).c_str(),
+     137             :                              getStateAsString(UNINITIALIZED_STATE).c_str());
+     138           0 :           return false;
+     139             :         }
+     140          94 :         break;
+     141             :       }
+     142             : 
+     143          94 :       case READY_FOR_FLIGHT_STATE: {
+     144          94 :         if (current_state_ != INITIALIZED_STATE && current_state_ != LANDED_STATE && current_state_ != ESTIMATOR_SWITCHING_STATE) {
+     145           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s, %s or %s", getPrintName().c_str(),
+     146             :                              getStateAsString(READY_FOR_FLIGHT_STATE).c_str(), getStateAsString(INITIALIZED_STATE).c_str(),
+     147             :                              getStateAsString(LANDED_STATE).c_str(), getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str());
+     148           0 :           return false;
+     149             :         }
+     150          94 :         break;
+     151             :       }
+     152             : 
+     153          22 :       case TAKING_OFF_STATE: {
+     154          22 :         if (current_state_ != READY_FOR_FLIGHT_STATE && current_state_ != ESTIMATOR_SWITCHING_STATE) {
+     155           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s", getPrintName().c_str(), getStateAsString(TAKING_OFF_STATE).c_str(),
+     156             :                              getStateAsString(READY_FOR_FLIGHT_STATE).c_str(), getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str());
+     157           0 :           return false;
+     158             :         }
+     159          22 :         break;
+     160             :       }
+     161             : 
+     162          89 :       case FLYING_STATE: {
+     163          89 :         if (current_state_ != TAKING_OFF_STATE && current_state_ != READY_FOR_FLIGHT_STATE && current_state_ != HOVER_STATE && current_state_ != ESTIMATOR_SWITCHING_STATE) {
+     164           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s or %s", getPrintName().c_str(), getStateAsString(FLYING_STATE).c_str(),
+     165             :                              getStateAsString(TAKING_OFF_STATE).c_str(), getStateAsString(HOVER_STATE).c_str(),
+     166             :                              getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str());
+     167           0 :           return false;
+     168             :         }
+     169          89 :         break;
+     170             :       }
+     171             : 
+     172           0 :       case HOVER_STATE: {
+     173           0 :         if (current_state_ != FLYING_STATE && current_state_ != ESTIMATOR_SWITCHING_STATE) {
+     174           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s", getPrintName().c_str(), getStateAsString(HOVER_STATE).c_str(),
+     175             :                              getStateAsString(FLYING_STATE).c_str(), getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str());
+     176           0 :           return false;
+     177             :         }
+     178           0 :         break;
+     179             :       }
+     180             : 
+     181           5 :       case ESTIMATOR_SWITCHING_STATE: {
+     182           5 :         if (current_state_ != READY_FOR_FLIGHT_STATE && current_state_ != TAKING_OFF_STATE  && current_state_ != HOVER_STATE && current_state_ != FLYING_STATE && current_state_ != LANDING_STATE) {
+     183           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s, %s, %s, %s or %s", getPrintName().c_str(),
+     184             :                              getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str(), getStateAsString(READY_FOR_FLIGHT_STATE).c_str(), getStateAsString(TAKING_OFF_STATE).c_str(), getStateAsString(FLYING_STATE).c_str(),
+     185             :                              getStateAsString(HOVER_STATE).c_str(), getStateAsString(FLYING_STATE).c_str());
+     186           0 :           return false;
+     187             :         }
+     188           5 :         pre_switch_state_ = current_state_;
+     189           5 :         break;
+     190             :       }
+     191             : 
+     192           0 :       case LANDING_STATE: {
+     193           0 :         if (current_state_ != FLYING_STATE && current_state_ != HOVER_STATE && current_state_ != ESTIMATOR_SWITCHING_STATE) {
+     194           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s, %s or %s", getPrintName().c_str(), getStateAsString(LANDING_STATE).c_str(),
+     195             :                              getStateAsString(FLYING_STATE).c_str(), getStateAsString(HOVER_STATE).c_str(), getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str());
+     196           0 :           return false;
+     197             :         }
+     198           0 :         break;
+     199             :       }
+     200             : 
+     201           0 :       case LANDED_STATE: {
+     202           0 :         if (current_state_ != LANDING_STATE) {
+     203           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(LANDED_STATE).c_str(),
+     204             :                              getStateAsString(LANDING_STATE).c_str());
+     205           0 :           return false;
+     206             :         }
+     207           0 :         break;
+     208             :       }
+     209             : 
+     210           0 :       case DUMMY_STATE: {
+     211           0 :         if (current_state_ != INITIALIZED_STATE) {
+     212           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(DUMMY_STATE).c_str(),
+     213             :                              getStateAsString(INITIALIZED_STATE).c_str());
+     214           0 :           return false;
+     215             :         }
+     216           0 :         break;
+     217             :       }
+     218           0 :       case EMERGENCY_STATE: {
+     219           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(EMERGENCY_STATE).c_str());
+     220           0 :         break;
+     221             :       }
+     222             : 
+     223           0 :       case ERROR_STATE: {
+     224           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(ERROR_STATE).c_str());
+     225           0 :         break;
+     226             :       }
+     227             : 
+     228           0 :       case FAILSAFE_STATE: {
+     229           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(FAILSAFE_STATE).c_str());
+     230           0 :         break;
+     231             :       }
+     232             : 
+     233           0 :       default: {
+     234           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: rejected transition to unknown state id %d", getPrintName().c_str(), target_state);
+     235           0 :         return false;
+     236             :         break;
+     237             :       }
+     238             :     }
+     239             : 
+     240         304 :     std::scoped_lock lock(mtx_state_);
+     241             :     {
+     242         304 :       previous_state_ = current_state_;
+     243         304 :       current_state_  = target_state;
+     244             :     }
+     245             : 
+     246         304 :     ROS_INFO("[%s]: successfully changed states %s -> %s", getPrintName().c_str(), getStateAsString(previous_state_).c_str(),
+     247             :              getStateAsString(current_state_).c_str());
+     248             : 
+     249         304 :     return true;
+     250             :   }
+     251             :   /*//}*/
+     252             : 
+     253             :   /*//{ changeToPreSwitchState() */
+     254           5 :   void changeToPreSwitchState() {
+     255           5 :     changeState(pre_switch_state_);
+     256           5 :   }
+     257             :   /*//}*/
+     258             : 
+     259             : private:
+     260             :   const std::string name_ = "StateMachine";
+     261             :   const std::string nodelet_name_;
+     262             : 
+     263             :   SMState_t current_state_    = UNINITIALIZED_STATE;
+     264             :   SMState_t previous_state_   = UNINITIALIZED_STATE;
+     265             :   SMState_t pre_switch_state_ = UNINITIALIZED_STATE;
+     266             : 
+     267             :   mutable std::mutex mtx_state_;
+     268             : 
+     269             :   std::string getName() const {
+     270             :     return name_;
+     271             :   }
+     272             : 
+     273         304 :   std::string getPrintName() const {
+     274         608 :     return nodelet_name_ + "/" + name_;
+     275             :   }
+     276             : 
+     277             :   // clang-format off
+     278             :   const std::vector<std::string> sm_state_names_ = {
+     279             :   "UNINITIALIZED_STATE",
+     280             :   "INITIALIZED_STATE",
+     281             :   "READY_FOR_FLIGHT_STATE",
+     282             :   "TAKING_OFF_STATE",
+     283             :   "FLYING_STATE",
+     284             :   "HOVER_STATE",
+     285             :   "LANDING_STATE",
+     286             :   "LANDED_STATE",
+     287             :   "ESTIMATOR_SWITCHING_STATE",
+     288             :   "DUMMY_STATE",
+     289             :   "EMERGENCY_STATE",
+     290             :   "FAILSAFE_STATE",
+     291             :   "ERROR_STATE"
+     292             :   };
+     293             :   // clang-format on
+     294             : };
+     295             : /*//}*/
+     296             : 
+     297             : /*//{ class EstimationManager */
+     298             : class EstimationManager : public nodelet::Nodelet {
+     299             : 
+     300             : private:
+     301             :   const std::string nodelet_name_ = "EstimationManager";
+     302             :   const std::string package_name_ = "mrs_uav_managers";
+     303             : 
+     304             :   ros::NodeHandle nh_;
+     305             : 
+     306             :   std::string _custom_config_;
+     307             :   std::string _platform_config_;
+     308             :   std::string _world_config_;
+     309             : 
+     310             :   std::shared_ptr<CommonHandlers_t> ch_;
+     311             : 
+     312             :   std::shared_ptr<StateMachine> sm_;
+     313             : 
+     314             :   std::string after_midair_activation_tracker_name_;
+     315             :   std::string takeoff_tracker_name_;
+     316             : 
+     317             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+     318             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>            sh_control_input_;
+     319             : 
+     320             :   mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics> ph_diagnostics_;
+     321             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>        ph_max_flight_z_;
+     322             :   mrs_lib::PublisherHandler<mrs_msgs::UavState>              ph_uav_state_;
+     323             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_odom_main_;
+     324             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_odom_slow_;  // use topic throttler instead?
+     325             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_innovation_;
+     326             : 
+     327             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_altitude_amsl_;
+     328             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_altitude_agl_;
+     329             : 
+     330             :   mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped> ph_orientation_;
+     331             : 
+     332             :   ros::Timer timer_publish_;
+     333             :   void       timerPublish(const ros::TimerEvent& event);
+     334             : 
+     335             :   ros::Timer timer_publish_diagnostics_;
+     336             :   void       timerPublishDiagnostics(const ros::TimerEvent& event);
+     337             : 
+     338             :   ros::Timer timer_check_health_;
+     339             :   void       timerCheckHealth(const ros::TimerEvent& event);
+     340             : 
+     341             :   ros::Timer timer_initialization_;
+     342             :   void       timerInitialization(const ros::TimerEvent& event);
+     343             : 
+     344             :   ros::ServiceServer srvs_change_estimator_;
+     345             :   bool               callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     346             :   int                estimator_switch_count_ = 0;
+     347             : 
+     348             : 
+     349             :   ros::ServiceServer srvs_toggle_callbacks_;
+     350             :   bool               callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     351             :   bool               callbacks_enabled_             = false;
+     352             :   bool               callbacks_disabled_by_service_ = false;
+     353             : 
+     354             :   bool                                             callFailsafeService();
+     355             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvch_failsafe_;
+     356             :   bool                                             failsafe_call_succeeded_ = false;
+     357             : 
+     358             :   // TODO service clients
+     359             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_hover_; */
+     360             :   /* mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> srvc_reference_; */
+     361             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_ehover_; */
+     362             :   /* mrs_lib::ServiceClientHandler<std_srvs::SetBool> srvc_enable_callbacks_; */
+     363             : 
+     364             :   // | ------------- dynamic loading of estimators ------------- |
+     365             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>> state_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     366             :   std::vector<std::string>                                                  estimator_names_;         // list of estimator names
+     367             :   std::vector<boost::shared_ptr<mrs_uav_managers::StateEstimator>>          estimator_list_;          // list of estimators
+     368             :   std::mutex                                                                mutex_estimator_list_;
+     369             :   std::vector<std::string>                                                  switchable_estimator_names_;
+     370             :   std::mutex                                                                mutex_switchable_estimator_names_;
+     371             :   std::string                                                               initial_estimator_name_ = "UNDEFINED_INITIAL_ESTIMATOR";
+     372             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       initial_estimator_;
+     373             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       active_estimator_;
+     374             :   std::mutex                                                                mutex_active_estimator_;
+     375             : 
+     376             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>> agl_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     377             :   std::string                                                             est_alt_agl_name_ = "UNDEFINED_AGL_ESTIMATOR";
+     378             :   boost::shared_ptr<mrs_uav_managers::AglEstimator>                       est_alt_agl_;
+     379             :   bool                                                                    is_using_agl_estimator_;
+     380             : 
+     381             :   double max_flight_z_;
+     382             : 
+     383             :   bool switchToHealthyEstimator();
+     384             :   void switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator);
+     385             : 
+     386             :   bool loadConfigFile(const std::string& file_path);
+     387             : 
+     388             : public:
+     389          94 :   EstimationManager() {
+     390          94 :   }
+     391             : 
+     392             :   void onInit();
+     393             : 
+     394             :   std::string getName() const;
+     395             : };
+     396             : /*//}*/
+     397             : 
+     398             : /*//{ onInit() */
+     399          94 : void EstimationManager::onInit() {
+     400             : 
+     401          94 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     402             : 
+     403          94 :   ros::Time::waitForValid();
+     404             : 
+     405          94 :   ROS_INFO("[%s]: initializing", getName().c_str());
+     406             : 
+     407          94 :   sm_ = std::make_shared<StateMachine>(nodelet_name_);
+     408             : 
+     409          94 :   ch_ = std::make_shared<CommonHandlers_t>();
+     410             : 
+     411          94 :   ch_->nodelet_name = nodelet_name_;
+     412          94 :   ch_->package_name = package_name_;
+     413             : 
+     414             :   // finish initialization in a oneshot timer, so that we don't block loading of other nodelets by the nodelet manager
+     415          94 :   timer_initialization_ = nh_.createTimer(ros::Rate(1.0), &EstimationManager::timerInitialization, this, true, true);
+     416          94 : }
+     417             : /*//}*/
+     418             : 
+     419             : // --------------------------------------------------------------
+     420             : // |                          callbacks                         |
+     421             : // --------------------------------------------------------------
+     422             : 
+     423             : // | --------------------- timer callbacks -------------------- |
+     424             : 
+     425             : /*//{ timerPublish() */
+     426      176475 : void EstimationManager::timerPublish([[maybe_unused]] const ros::TimerEvent& event) {
+     427             : 
+     428      176475 :   if (!sm_->isInitialized()) {
+     429       17845 :     return;
+     430             :   }
+     431             : 
+     432      352950 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublish", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     433             : 
+     434      176475 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     435           0 :     ROS_WARN("[%s]: Not publishing during estimator switching.", getName().c_str());
+     436           0 :     return;
+     437             :   }
+     438             : 
+     439      176475 :   if (!sm_->isInPublishableState()) {
+     440       17845 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     441       17845 :     return;
+     442             :   }
+     443             : 
+     444      158630 :   mrs_msgs::UavState uav_state;
+     445      158630 :   auto               ret = active_estimator_->getUavState();
+     446      158630 :   if (ret) {
+     447      158630 :     uav_state = ret.value();
+     448             :   } else {
+     449           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     450           0 :     return;
+     451             :   }
+     452             : 
+     453      158630 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     454           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     455           0 :     return;
+     456             :   }
+     457             : 
+     458             : 
+     459      158630 :   if (active_estimator_->isMitigatingJump()) {
+     460           0 :     estimator_switch_count_++;
+     461             :   }
+     462      158630 :   uav_state.estimator_iteration = estimator_switch_count_;
+     463             : 
+     464      158630 :   scope_timer.checkpoint("get uav state");
+     465             :   // TODO state health checks
+     466             : 
+     467      158630 :   ph_uav_state_.publish(uav_state);
+     468             : 
+     469      158630 :   scope_timer.checkpoint("pub uav state");
+     470      317260 :   nav_msgs::Odometry odom_main = Support::uavStateToOdom(uav_state);
+     471             : 
+     472      158630 :   scope_timer.checkpoint("uav state to odom");
+     473      317260 :   const std::vector<double> pose_covariance = active_estimator_->getPoseCovariance();
+     474     5869304 :   for (size_t i = 0; i < pose_covariance.size(); i++) {
+     475     5710682 :     odom_main.pose.covariance[i] = pose_covariance[i];
+     476             :   }
+     477             : 
+     478      317260 :   const std::vector<double> twist_covariance = active_estimator_->getTwistCovariance();
+     479     5869304 :   for (size_t i = 0; i < twist_covariance.size(); i++) {
+     480     5710682 :     odom_main.twist.covariance[i] = twist_covariance[i];
+     481             :   }
+     482             : 
+     483      158630 :   scope_timer.checkpoint("get covariance");
+     484      158630 :   ph_odom_main_.publish(odom_main);
+     485             : 
+     486      317260 :   nav_msgs::Odometry innovation = active_estimator_->getInnovation();
+     487      158630 :   ph_innovation_.publish(innovation);
+     488             : 
+     489             : 
+     490      317260 :   mrs_msgs::Float64Stamped agl_height;
+     491             : 
+     492      158630 :   if (is_using_agl_estimator_) {
+     493      128534 :     agl_height = est_alt_agl_->getUavAglHeight();
+     494      128534 :     ph_altitude_agl_.publish(agl_height);
+     495             :   }
+     496             : }
+     497             : /*//}*/
+     498             : 
+     499             : /*//{ timerPublishDiagnostics() */
+     500       18974 : void EstimationManager::timerPublishDiagnostics([[maybe_unused]] const ros::TimerEvent& event) {
+     501             : 
+     502       18974 :   if (!sm_->isInitialized()) {
+     503        1778 :     return;
+     504             :   }
+     505             : 
+     506       37948 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublishDiagnostics", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     507             : 
+     508       18974 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     509           0 :     ROS_WARN("[%s]: Not publishing diagnostics during estimator switching.", getName().c_str());
+     510           0 :     return;
+     511             :   }
+     512             : 
+     513       18974 :   if (!sm_->isInPublishableState()) {
+     514        1778 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     515        1778 :     return;
+     516             :   }
+     517             : 
+     518       17196 :   mrs_msgs::UavState uav_state;
+     519       17196 :   auto               ret = active_estimator_->getUavState();
+     520       17196 :   if (ret) {
+     521       17196 :     uav_state = ret.value();
+     522             :   } else {
+     523           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     524           0 :     return;
+     525             :   }
+     526             : 
+     527       17196 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     528           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     529           0 :     return;
+     530             :   }
+     531             : 
+     532       34392 :   mrs_msgs::Float64Stamped agl_height;
+     533             : 
+     534       17196 :   if (is_using_agl_estimator_) {
+     535       13018 :     agl_height = est_alt_agl_->getUavAglHeight();
+     536       13018 :     ph_altitude_agl_.publish(agl_height);
+     537             :   }
+     538             : 
+     539       34392 :   mrs_msgs::Float64Stamped max_flight_z_msg;
+     540       17196 :   max_flight_z_msg.header.stamp = ros::Time::now();
+     541       17196 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     542       17196 :     max_flight_z_msg.header.frame_id = active_estimator_->getFrameId();
+     543       17196 :     max_flight_z_msg.value           = active_estimator_->getMaxFlightZ();
+     544             :   }
+     545       17196 :   max_flight_z_ = max_flight_z_msg.value;
+     546       17196 :   ph_max_flight_z_.publish(max_flight_z_msg);
+     547             : 
+     548             :   // publish diagnostics
+     549       34392 :   mrs_msgs::EstimationDiagnostics diagnostics;
+     550             : 
+     551       17196 :   diagnostics.header.stamp   = uav_state.header.stamp;
+     552       17196 :   diagnostics.child_frame_id = uav_state.child_frame_id;
+     553             : 
+     554       17196 :   diagnostics.pose         = uav_state.pose;
+     555       17196 :   diagnostics.velocity     = uav_state.velocity;
+     556       17196 :   diagnostics.acceleration = uav_state.acceleration;
+     557             : 
+     558       17196 :   diagnostics.sm_state                 = sm_->getCurrentStateString();
+     559       17196 :   diagnostics.max_flight_z             = max_flight_z_msg.value;
+     560       17196 :   diagnostics.estimator_iteration      = estimator_switch_count_;
+     561       17196 :   diagnostics.estimation_rate          = ch_->desired_uav_state_rate;
+     562       17196 :   diagnostics.running_state_estimators = estimator_names_;
+     563             : 
+     564             :   {
+     565       34392 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     566       17196 :     diagnostics.switchable_state_estimators = switchable_estimator_names_;
+     567             :   }
+     568             : 
+     569             : 
+     570       17196 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     571       17196 :     diagnostics.header.frame_id         = active_estimator_->getFrameId();
+     572       17196 :     diagnostics.current_state_estimator = active_estimator_->getName();
+     573             :   } else {
+     574           0 :     diagnostics.header.frame_id         = "";
+     575           0 :     diagnostics.current_state_estimator = "";
+     576             :   }
+     577             : 
+     578       17196 :   diagnostics.estimator_horizontal = uav_state.estimator_horizontal;
+     579       17196 :   diagnostics.estimator_vertical   = uav_state.estimator_vertical;
+     580       17196 :   diagnostics.estimator_heading    = uav_state.estimator_heading;
+     581             : 
+     582       17196 :   if (is_using_agl_estimator_) {
+     583       13018 :     diagnostics.estimator_agl_height = est_alt_agl_name_;
+     584       13018 :     diagnostics.agl_height           = agl_height.value;
+     585             :   } else {
+     586        4178 :     diagnostics.estimator_agl_height = "NOT_USED";
+     587        4178 :     diagnostics.agl_height           = std::nanf("");
+     588             :   }
+     589             : 
+     590       17196 :   diagnostics.platform_config = _platform_config_;
+     591       17196 :   diagnostics.custom_config   = _custom_config_;
+     592             : 
+     593       17196 :   ph_diagnostics_.publish(diagnostics);
+     594             : 
+     595       17196 :   ROS_INFO_THROTTLE(5.0, "[%s]: %s. pos: [%.2f, %.2f, %.2f] m. Estimator: %s. Max. z.: %.2f m. Estimator switches: %d.", getName().c_str(),
+     596             :                     sm_->getCurrentStateString().c_str(), uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z,
+     597             :                     active_estimator_->getName().c_str(), max_flight_z_, estimator_switch_count_);
+     598             : }
+     599             : /*//}*/
+     600             : 
+     601             : /*//{ timerCheckHealth() */
+     602      176475 : void EstimationManager::timerCheckHealth([[maybe_unused]] const ros::TimerEvent& event) {
+     603             : 
+     604      176475 :   if (!sm_->isInitialized()) {
+     605           0 :     return;
+     606             :   }
+     607             : 
+     608      529425 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     609             : 
+     610             :   /*//{ start ready estimators, check switchable estimators */
+     611      352950 :   std::vector<std::string> switchable_estimator_names;
+     612      370431 :   for (auto estimator : estimator_list_) {
+     613             : 
+     614      193956 :     if (estimator->isReady()) {
+     615             :       try {
+     616        7240 :         ROS_INFO_THROTTLE(1.0, "[%s]: starting the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     617        7240 :         estimator->start();
+     618             :       }
+     619           0 :       catch (std::runtime_error& ex) {
+     620           0 :         ROS_ERROR("[%s]: exception caught during estimator starting: '%s'", getName().c_str(), ex.what());
+     621           0 :         ros::shutdown();
+     622             :       }
+     623             :     }
+     624             : 
+     625      193956 :     if (estimator->isRunning() && estimator->getName() != "dummy" && estimator->getName() != "ground_truth") {
+     626      184533 :       switchable_estimator_names.push_back(estimator->getName());
+     627             :     }
+     628             :   }
+     629             : 
+     630             :   {
+     631      352950 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     632      176475 :     switchable_estimator_names_ = switchable_estimator_names;
+     633             :   }
+     634             : 
+     635      176475 :   if (is_using_agl_estimator_ && est_alt_agl_->isReady()) {
+     636          73 :     est_alt_agl_->start();
+     637             :   }
+     638             : 
+     639             :   /*//}*/
+     640             : 
+     641      321008 :   if (!callbacks_disabled_by_service_ &&
+     642      321008 :       (sm_->isInState(StateMachine::FLYING_STATE) || sm_->isInState(StateMachine::HOVER_STATE) || sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE))) {
+     643      126253 :     callbacks_enabled_ = true;
+     644             :   } else {
+     645       50222 :     callbacks_enabled_ = false;
+     646             :   }
+     647             : 
+     648             :   // TODO fuj if, zmenit na switch
+     649             :   // activate initial estimator
+     650      176475 :   if (sm_->isInState(StateMachine::INITIALIZED_STATE) && initial_estimator_->isRunning()) {
+     651       17162 :     std::scoped_lock lock(mutex_active_estimator_);
+     652        8581 :     ROS_INFO_THROTTLE(1.0, "[%s]: activating the initial estimator %s", getName().c_str(), initial_estimator_->getName().c_str());
+     653        8581 :     active_estimator_ = initial_estimator_;
+     654        8581 :     if (active_estimator_->getName() == "dummy") {
+     655           0 :       sm_->changeState(StateMachine::DUMMY_STATE);
+     656             :     } else {
+     657        8581 :       if (!is_using_agl_estimator_ || est_alt_agl_->isRunning()) {
+     658          94 :         sm_->changeState(StateMachine::READY_FOR_FLIGHT_STATE);
+     659             :       } else {
+     660        8487 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s agl estimator: %s to be running", getName().c_str(), Support::waiting_for_string.c_str(),
+     661             :                           est_alt_agl_->getName().c_str());
+     662             :       }
+     663             :     }
+     664             :   }
+     665             : 
+     666             :   // active estimator is in faulty state, we need to switch to healthy estimator
+     667      176475 :   if (sm_->isInSwitchableState() && active_estimator_->isError()) {
+     668           0 :     sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+     669             :   }
+     670             : 
+     671      176475 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     672           0 :     if (switchToHealthyEstimator()) {
+     673           0 :       sm_->changeToPreSwitchState();
+     674             :     } else {  // cannot switch to healthy estimator - failsafe necessary
+     675           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Cannot switch to any healthy estimator. Triggering failsafe.", getName().c_str());
+     676           0 :       sm_->changeState(StateMachine::FAILSAFE_STATE);
+     677             :     }
+     678             :   }
+     679             : 
+     680      176475 :   if (sm_->isInState(StateMachine::FAILSAFE_STATE)) {
+     681           0 :     if (!failsafe_call_succeeded_ && callFailsafeService()) {
+     682           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: failsafe called successfully", getName().c_str());
+     683           0 :       failsafe_call_succeeded_ = true;
+     684             :     }
+     685           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: we are in failsafe state", getName().c_str());
+     686             :   }
+     687             : 
+     688             :   // standard takeoff
+     689      176475 :   if (sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE)) {
+     690       41122 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == takeoff_tracker_name_) {
+     691          22 :       sm_->changeState(StateMachine::TAKING_OFF_STATE);
+     692             :     }
+     693             :   }
+     694             : 
+     695             :   // midair activation
+     696      176475 :   if (sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE)) {
+     697       41100 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == after_midair_activation_tracker_name_) {
+     698          64 :       sm_->changeState(StateMachine::FLYING_STATE);
+     699             :     }
+     700             :   }
+     701             : 
+     702      176475 :   if (sm_->isInState(StateMachine::TAKING_OFF_STATE)) {
+     703       10616 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker != takeoff_tracker_name_) {
+     704          20 :       sm_->changeState(StateMachine::FLYING_STATE);
+     705             :     }
+     706             :   }
+     707             : 
+     708      176475 :   if (sm_->isInState(StateMachine::FLYING_STATE)) {
+     709      107071 :     if (!sh_control_input_.hasMsg()) {
+     710        4082 :       ROS_WARN_THROTTLE(1.0, "[%s]: not received control input since starting EstimationManager, estimation suboptimal, potentially unstable", getName().c_str());
+     711      102989 :     } else if ((ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     712       13177 :       ROS_WARN_THROTTLE(1.0, "[%s]: not received control input for %.4fs, estimation suboptimal, potentially unstable", getName().c_str(),
+     713             :                         (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     714             :     }
+     715             :   }
+     716             : }
+     717             : /*//}*/
+     718             : 
+     719             : /*//{ timerInitialization() */
+     720          94 : void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEvent& event) {
+     721             : 
+     722         188 :   mrs_lib::ParamLoader param_loader(nh_, getName());
+     723             : 
+     724          94 :   param_loader.loadParam("custom_config", _custom_config_);
+     725          94 :   param_loader.loadParam("platform_config", _platform_config_);
+     726          94 :   param_loader.loadParam("world_config", _world_config_);
+     727             : 
+     728          94 :   if (_custom_config_ != "") {
+     729          94 :     param_loader.addYamlFile(_custom_config_);
+     730             :   }
+     731             : 
+     732          94 :   if (_platform_config_ != "") {
+     733          94 :     param_loader.addYamlFile(_platform_config_);
+     734             :   }
+     735             : 
+     736          94 :   if (_world_config_ != "") {
+     737          94 :     param_loader.addYamlFile(_world_config_);
+     738             :   }
+     739             : 
+     740          94 :   param_loader.addYamlFileFromParam("private_config");
+     741          94 :   param_loader.addYamlFileFromParam("public_config");
+     742          94 :   param_loader.addYamlFileFromParam("uav_manager_config");
+     743          94 :   param_loader.addYamlFileFromParam("estimators_config");
+     744          94 :   param_loader.addYamlFileFromParam("active_estimators_config");
+     745             : 
+     746          94 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     747             : 
+     748             :   /*//{ load world_origin parameters */
+     749             : 
+     750         188 :   std::string world_origin_units;
+     751          94 :   bool        is_origin_param_ok = true;
+     752          94 :   double      world_origin_x     = 0;
+     753          94 :   double      world_origin_y     = 0;
+     754             : 
+     755          94 :   param_loader.loadParam("world_origin/units", world_origin_units);
+     756             : 
+     757          94 :   if (Support::toLowercase(world_origin_units) == "utm") {
+     758           0 :     ROS_INFO("[%s]: Loading world origin in UTM units.", getName().c_str());
+     759           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", world_origin_x);
+     760           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", world_origin_y);
+     761             : 
+     762          94 :   } else if (Support::toLowercase(world_origin_units) == "latlon") {
+     763             :     double lat, lon;
+     764          94 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getName().c_str());
+     765          94 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     766          94 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     767          94 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     768          94 :     ROS_INFO("[%s]: Converted to UTM x: %f, y: %f.", getName().c_str(), world_origin_x, world_origin_y);
+     769             : 
+     770             :   } else {
+     771           0 :     ROS_ERROR("[%s]: world_origin_units must be (\"UTM\"|\"LATLON\"). Got '%s'", getName().c_str(), world_origin_units.c_str());
+     772           0 :     ros::shutdown();
+     773             :   }
+     774             : 
+     775          94 :   ch_->world_origin.x = world_origin_x;
+     776          94 :   ch_->world_origin.y = world_origin_y;
+     777             : 
+     778          94 :   if (!is_origin_param_ok) {
+     779           0 :     ROS_ERROR("[%s]: Could not load all mandatory parameters from world file. Please check your world file.", getName().c_str());
+     780           0 :     ros::shutdown();
+     781             :   }
+     782             :   /*//}*/
+     783             : 
+     784             :   /*//{ load tracker names */
+     785          94 :   param_loader.loadParam("mrs_uav_managers/uav_manager/takeoff/during_takeoff/tracker", takeoff_tracker_name_);
+     786          94 :   param_loader.loadParam("mrs_uav_managers/uav_manager/midair_activation/after_activation/tracker", after_midair_activation_tracker_name_);
+     787             :   /*//}*/
+     788             : 
+     789          94 :   param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/");
+     790             : 
+     791             :   /*//{ load parameters into common handlers */
+     792          94 :   param_loader.loadParam("frame_id/fcu", ch_->frames.fcu);
+     793          94 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + ch_->frames.fcu;
+     794             : 
+     795          94 :   param_loader.loadParam("frame_id/fcu_untilted", ch_->frames.fcu_untilted);
+     796          94 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + ch_->frames.fcu_untilted;
+     797             : 
+     798          94 :   param_loader.loadParam("frame_id/rtk_antenna", ch_->frames.rtk_antenna);
+     799          94 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     800             : 
+     801          94 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getName());
+     802          94 :   ch_->transformer->retryLookupNewest(true);
+     803             : 
+     804          94 :   param_loader.loadParam("rate/diagnostics", ch_->desired_diagnostics_rate);
+     805             : 
+     806             :   /*//}*/
+     807             : 
+     808             :   /*//{ load parameters */
+     809             : 
+     810             :   /*//{ publish debug topics parameters */
+     811          94 :   param_loader.loadParam("debug_topics/input", ch_->debug_topics.input);
+     812          94 :   param_loader.loadParam("debug_topics/output", ch_->debug_topics.output);
+     813          94 :   param_loader.loadParam("debug_topics/state", ch_->debug_topics.state);
+     814          94 :   param_loader.loadParam("debug_topics/covariance", ch_->debug_topics.covariance);
+     815          94 :   param_loader.loadParam("debug_topics/innovation", ch_->debug_topics.innovation);
+     816          94 :   param_loader.loadParam("debug_topics/diagnostics", ch_->debug_topics.diag);
+     817          94 :   param_loader.loadParam("debug_topics/correction", ch_->debug_topics.correction);
+     818          94 :   param_loader.loadParam("debug_topics/correction_delay", ch_->debug_topics.corr_delay);
+     819             :   /*//}*/
+     820             : 
+     821             :   /*//}*/
+     822             : 
+     823         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+     824          94 :   shopts.nh                 = nh_;
+     825          94 :   shopts.node_name          = getName();
+     826          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     827          94 :   shopts.threadsafe         = true;
+     828          94 :   shopts.autostart          = true;
+     829          94 :   shopts.queue_size         = 10;
+     830          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     831             : 
+     832             :   /*//{ wait for hw api message */
+     833             : 
+     834             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_ =
+     835         282 :       mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     836         193 :   while (!sh_hw_api_capabilities_.hasMsg()) {
+     837          99 :     ROS_INFO("[%s]: %s hw_api_capabilities message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     838             :              sh_hw_api_capabilities_.topicName().c_str());
+     839          99 :     ros::Duration(1.0).sleep();
+     840             :   }
+     841             : 
+     842         188 :   mrs_msgs::HwApiCapabilitiesConstPtr hw_api_capabilities = sh_hw_api_capabilities_.getMsg();
+     843             :   /*//}*/
+     844             : 
+     845             :   /*//{ wait for desired uav_state rate */
+     846          94 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     847         198 :   while (!sh_control_manager_diag_.hasMsg()) {
+     848         104 :     ROS_INFO("[%s]: %s control_manager_diagnostics message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     849             :              sh_control_manager_diag_.topicName().c_str());
+     850         104 :     ros::Duration(1.0).sleep();
+     851             :   }
+     852             : 
+     853         188 :   mrs_msgs::ControlManagerDiagnosticsConstPtr control_manager_diag_msg = sh_control_manager_diag_.getMsg();
+     854          94 :   ch_->desired_uav_state_rate                                          = control_manager_diag_msg->desired_uav_state_rate;
+     855          94 :   ROS_INFO("[%s]: The estimation is running at: %.2f Hz", getName().c_str(), ch_->desired_uav_state_rate);
+     856             :   /*//}*/
+     857             : 
+     858             :   /*//{ initialize subscribers */
+     859             : 
+     860          94 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     861             : 
+     862             :   /*//}*/
+     863             : 
+     864             :   /*//{ load state estimator plugins */
+     865          94 :   param_loader.loadParam("state_estimators", estimator_names_);
+     866             : 
+     867          94 :   state_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>>("mrs_uav_managers", "mrs_uav_managers::StateEstimator");
+     868             : 
+     869         194 :   for (size_t i = 0; i < estimator_names_.size(); i++) {
+     870             : 
+     871         200 :     const std::string estimator_name = estimator_names_[i];
+     872             : 
+     873             :     // load the estimator parameters
+     874         200 :     std::string address;
+     875         100 :     param_loader.loadParam(estimator_name + "/address", address);
+     876             : 
+     877             :     try {
+     878         100 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     879         100 :       estimator_list_.push_back(state_estimator_loader_->createInstance(address.c_str()));
+     880             :     }
+     881           0 :     catch (pluginlib::CreateClassException& ex1) {
+     882           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     883           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     884           0 :       ros::shutdown();
+     885             :     }
+     886           0 :     catch (pluginlib::PluginlibException& ex) {
+     887           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     888           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     889           0 :       ros::shutdown();
+     890             :     }
+     891             :   }
+     892             : 
+     893             :   /*//{ load agl estimator plugins */
+     894          94 :   param_loader.loadParam("agl_height_estimator", est_alt_agl_name_);
+     895          94 :   is_using_agl_estimator_ = est_alt_agl_name_ != "";
+     896          94 :   ROS_WARN_COND(!is_using_agl_estimator_, "[%s]: not using AGL estimator for min height safe checking", getName().c_str());
+     897             : 
+     898             : 
+     899          94 :   if (is_using_agl_estimator_) {
+     900             : 
+     901          73 :     agl_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>>("mrs_uav_managers", "mrs_uav_managers::AglEstimator");
+     902             : 
+     903             :     // load the estimator parameters
+     904         146 :     std::string address;
+     905          73 :     param_loader.loadParam(est_alt_agl_name_ + "/address", address);
+     906             : 
+     907             :     try {
+     908          73 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     909          73 :       est_alt_agl_ = agl_estimator_loader_->createInstance(address.c_str());
+     910             :     }
+     911           0 :     catch (pluginlib::CreateClassException& ex1) {
+     912           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     913           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     914           0 :       ros::shutdown();
+     915             :     }
+     916           0 :     catch (pluginlib::PluginlibException& ex) {
+     917           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     918           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     919           0 :       ros::shutdown();
+     920             :     }
+     921             :   }
+     922             :   /*//}*/
+     923             : 
+     924          94 :   ROS_INFO("[%s]: estimators were loaded", getName().c_str());
+     925             :   /*//}*/
+     926             : 
+     927             :   /*//{ check whether initial estimator was loaded */
+     928          94 :   param_loader.loadParam("initial_state_estimator", initial_estimator_name_);
+     929          94 :   bool initial_estimator_found = false;
+     930          94 :   for (auto estimator : estimator_list_) {
+     931          94 :     if (estimator->getName() == initial_estimator_name_) {
+     932          94 :       initial_estimator_      = estimator;
+     933          94 :       initial_estimator_found = true;
+     934          94 :       break;
+     935             :     }
+     936             :   }
+     937             : 
+     938          94 :   if (!initial_estimator_found) {
+     939           0 :     ROS_ERROR("[%s]: initial estimator %s could not be found among loaded estimators. shutting down", getName().c_str(), initial_estimator_name_.c_str());
+     940           0 :     ros::shutdown();
+     941             :   }
+     942             :   /*//}*/
+     943             : 
+     944             :   /*//{ initialize estimators */
+     945         194 :   for (auto estimator : estimator_list_) {
+     946             : 
+     947             :     // create private handlers
+     948         200 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     949             : 
+     950         100 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     951         100 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, estimator->getName()), "EstimationManager/" + estimator->getName());
+     952             : 
+     953         100 :     if (_custom_config_ != "") {
+     954         100 :       ph->param_loader->addYamlFile(_custom_config_);
+     955             :     }
+     956             : 
+     957         100 :     if (_platform_config_ != "") {
+     958         100 :       ph->param_loader->addYamlFile(_platform_config_);
+     959             :     }
+     960             : 
+     961         100 :     if (_world_config_ != "") {
+     962         100 :       ph->param_loader->addYamlFile(_world_config_);
+     963             :     }
+     964             : 
+     965             :     try {
+     966         100 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     967         100 :       estimator->initialize(nh_, ch_, ph);
+     968             :     }
+     969           0 :     catch (std::runtime_error& ex) {
+     970           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+     971           0 :       ros::shutdown();
+     972             :     }
+     973             : 
+     974         100 :     if (!estimator->isCompatibleWithHwApi(hw_api_capabilities)) {
+     975           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), estimator->getName().c_str());
+     976           0 :       ros::shutdown();
+     977             :     }
+     978             :   }
+     979             : 
+     980             :   // | ----------- agl height estimator initialization ---------- |
+     981          94 :   if (is_using_agl_estimator_) {
+     982             : 
+     983         146 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     984             : 
+     985          73 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     986          73 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, est_alt_agl_->getName()), "EstimationManager/" + est_alt_agl_->getName());
+     987             : 
+     988          73 :     if (_custom_config_ != "") {
+     989          73 :       ph->param_loader->addYamlFile(_custom_config_);
+     990             :     }
+     991             : 
+     992          73 :     if (_platform_config_ != "") {
+     993          73 :       ph->param_loader->addYamlFile(_platform_config_);
+     994             :     }
+     995             : 
+     996          73 :     if (_world_config_ != "") {
+     997          73 :       ph->param_loader->addYamlFile(_world_config_);
+     998             :     }
+     999             : 
+    1000             :     try {
+    1001          73 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), est_alt_agl_->getName().c_str());
+    1002          73 :       est_alt_agl_->initialize(nh_, ch_, ph);
+    1003             :     }
+    1004           0 :     catch (std::runtime_error& ex) {
+    1005           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+    1006           0 :       ros::shutdown();
+    1007             :     }
+    1008             : 
+    1009          73 :     if (!est_alt_agl_->isCompatibleWithHwApi(hw_api_capabilities)) {
+    1010           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), est_alt_agl_->getName().c_str());
+    1011           0 :       ros::shutdown();
+    1012             :     }
+    1013             :   }
+    1014             : 
+    1015          94 :   ROS_INFO("[%s]: estimators were initialized", getName().c_str());
+    1016             : 
+    1017             :   /*//}*/
+    1018             : 
+    1019             :   /*//{ initialize publishers */
+    1020          94 :   ph_uav_state_    = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh_, "uav_state_out", 10);
+    1021          94 :   ph_odom_main_    = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "odom_main_out", 10);
+    1022          94 :   ph_innovation_   = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "innovation_out", 10);
+    1023          94 :   ph_diagnostics_  = mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics>(nh_, "diagnostics_out", 10);
+    1024          94 :   ph_max_flight_z_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "max_flight_z_agl_out", 10);
+    1025          94 :   ph_altitude_agl_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "height_agl_out", 10);
+    1026             : 
+    1027             :   /*//}*/
+    1028             : 
+    1029             :   /*//{ initialize timers */
+    1030          94 :   timer_publish_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerPublish, this);
+    1031             : 
+    1032          94 :   timer_publish_diagnostics_ = nh_.createTimer(ros::Rate(ch_->desired_diagnostics_rate), &EstimationManager::timerPublishDiagnostics, this);
+    1033             : 
+    1034          94 :   timer_check_health_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerCheckHealth, this);
+    1035             :   /*//}*/
+    1036             : 
+    1037             :   /*//{ initialize service clients */
+    1038             : 
+    1039          94 :   srvch_failsafe_.initialize(nh_, "failsafe_out");
+    1040             : 
+    1041             :   /*//}*/
+    1042             : 
+    1043             :   /*//{ initialize service servers */
+    1044          94 :   srvs_change_estimator_ = nh_.advertiseService("change_estimator_in", &EstimationManager::callbackChangeEstimator, this);
+    1045          94 :   srvs_toggle_callbacks_ = nh_.advertiseService("toggle_service_callbacks_in", &EstimationManager::callbackToggleServiceCallbacks, this);
+    1046             :   /*//}*/
+    1047             : 
+    1048             :   /*//{ initialize scope timer */
+    1049          94 :   param_loader.loadParam("scope_timer/enabled", ch_->scope_timer.enabled);
+    1050         188 :   std::string       filepath;
+    1051         188 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/scope_timer.txt";
+    1052          94 :   ch_->scope_timer.logger                = std::make_shared<mrs_lib::ScopeTimerLogger>(time_logger_filepath, ch_->scope_timer.enabled);
+    1053             :   /*//}*/
+    1054             : 
+    1055          94 :   if (!param_loader.loadedSuccessfully()) {
+    1056           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getName().c_str());
+    1057           0 :     ros::shutdown();
+    1058             :   }
+    1059             : 
+    1060          94 :   sm_->changeState(StateMachine::INITIALIZED_STATE);
+    1061             : 
+    1062          94 :   ROS_INFO("[%s]: initialized", getName().c_str());
+    1063          94 : }
+    1064             : /*//}*/
+    1065             : 
+    1066             : // | -------------------- service callbacks ------------------- |
+    1067             : 
+    1068             : /*//{ callbackChangeEstimator() */
+    1069           5 : bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    1070             : 
+    1071           5 :   if (!sm_->isInitialized()) {
+    1072           0 :     return false;
+    1073             :   }
+    1074             : 
+    1075           5 :   if (!callbacks_enabled_) {
+    1076           0 :     res.success = false;
+    1077           0 :     res.message = ("Service callbacks are disabled");
+    1078           0 :     ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
+    1079           0 :     return true;
+    1080             :   }
+    1081             : 
+    1082           5 :   if (req.value == "dummy" || req.value == "ground_truth") {
+    1083           0 :     res.success = false;
+    1084           0 :     std::stringstream ss;
+    1085           0 :     ss << "Switching to " << req.value << " estimator is not allowed.";
+    1086           0 :     res.message = ss.str();
+    1087           0 :     ROS_WARN("[%s]: Switching to %s estimator is not allowed.", getName().c_str(), req.value.c_str());
+    1088           0 :     return true;
+    1089             :   }
+    1090             : 
+    1091             :   // we do not want the switch to be disturbed by a service call
+    1092           5 :   callbacks_enabled_ = false;
+    1093             : 
+    1094           5 :   bool                                                target_estimator_found = false;
+    1095           5 :   boost::shared_ptr<mrs_uav_managers::StateEstimator> target_estimator;
+    1096          12 :   for (auto estimator : estimator_list_) {
+    1097          12 :     if (estimator->getName() == req.value) {
+    1098           5 :       target_estimator       = estimator;
+    1099           5 :       target_estimator_found = true;
+    1100           5 :       break;
+    1101             :     }
+    1102             :   }
+    1103             : 
+    1104           5 :   if (target_estimator_found) {
+    1105             : 
+    1106           5 :     if (target_estimator->isRunning()) {
+    1107           5 :       sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+    1108           5 :       switchToEstimator(target_estimator);
+    1109           5 :       sm_->changeToPreSwitchState();
+    1110             :     } else {
+    1111           0 :       ROS_WARN("[%s]: Switch to not running estimator %s requested", getName().c_str(), req.value.c_str());
+    1112           0 :       res.success = false;
+    1113           0 :       res.message = ("Requested estimator is not running");
+    1114           0 :       return true;
+    1115             :     }
+    1116             : 
+    1117             :   } else {
+    1118           0 :     ROS_WARN("[%s]: Switch to invalid estimator %s requested", getName().c_str(), req.value.c_str());
+    1119           0 :     res.success = false;
+    1120           0 :     res.message = ("Not a valid estimator type");
+    1121           0 :     return true;
+    1122             :   }
+    1123             : 
+    1124           5 :   res.success = true;
+    1125           5 :   res.message = "Estimator switch successful";
+    1126             : 
+    1127             :   // allow service calllbacks after switch again
+    1128           5 :   callbacks_enabled_ = true;
+    1129             : 
+    1130           5 :   return true;
+    1131             : }
+    1132             : /*//}*/
+    1133             : 
+    1134             : /* //{ callbackToggleServiceCallbacks() */
+    1135         128 : bool EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1136             : 
+    1137         128 :   if (!sm_->isInitialized()) {
+    1138           0 :     ROS_ERROR("[%s]: service for toggling callbacks is not available before initialization.", getName().c_str());
+    1139           0 :     return false;
+    1140             :   }
+    1141             : 
+    1142         128 :   callbacks_disabled_by_service_ = !req.data;
+    1143             : 
+    1144         128 :   res.success = true;
+    1145         128 :   res.message = (callbacks_disabled_by_service_ ? "Service callbacks disabled" : "Service callbacks enabled");
+    1146             : 
+    1147         128 :   if (callbacks_disabled_by_service_) {
+    1148             : 
+    1149          42 :     ROS_INFO("[%s]: Service callbacks disabled.", getName().c_str());
+    1150             : 
+    1151             :   } else {
+    1152             : 
+    1153          86 :     ROS_INFO("[%s]: Service callbacks enabled", getName().c_str());
+    1154             :   }
+    1155             : 
+    1156         128 :   return true;
+    1157             : }
+    1158             : 
+    1159             : //}
+    1160             : 
+    1161             : 
+    1162             : // --------------------------------------------------------------
+    1163             : // |                        other methods                       |
+    1164             : // --------------------------------------------------------------
+    1165             : //
+    1166             : /*//{ switchToHealthyEstimator() */
+    1167           0 : bool EstimationManager::switchToHealthyEstimator() {
+    1168             : 
+    1169             :   // available estimators should be specified in decreasing priority order in config file
+    1170           0 :   for (auto estimator : estimator_list_) {
+    1171           0 :     if (estimator->isRunning()) {
+    1172           0 :       switchToEstimator(estimator);
+    1173           0 :       return true;
+    1174             :     }
+    1175             :   }
+    1176           0 :   return false;  // no other estimator is running
+    1177             : }
+    1178             : /*//}*/
+    1179             : 
+    1180             : /*//{ switchToEstimator() */
+    1181           5 : void EstimationManager::switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator) {
+    1182             : 
+    1183           5 :   std::scoped_lock lock(mutex_active_estimator_);
+    1184           5 :   ROS_INFO("[%s]: switching estimator from %s to %s", getName().c_str(), active_estimator_->getName().c_str(), target_estimator->getName().c_str());
+    1185           5 :   active_estimator_ = target_estimator;
+    1186           5 :   estimator_switch_count_++;
+    1187           5 : }
+    1188             : /*//}*/
+    1189             : 
+    1190             : /*//{ callFailsafeService() */
+    1191           0 : bool EstimationManager::callFailsafeService() {
+    1192           0 :   std_srvs::Trigger srv_out;
+    1193           0 :   return srvch_failsafe_.call(srv_out);
+    1194             : }
+    1195             : /*//}*/
+    1196             : 
+    1197             : /*//{ getName() */
+    1198        3162 : std::string EstimationManager::getName() const {
+    1199        3162 :   return nodelet_name_;
+    1200             : }
+    1201             : /*//}*/
+    1202             : 
+    1203             : /* loadConfigFile() //{ */
+    1204             : 
+    1205           0 : bool EstimationManager::loadConfigFile(const std::string& file_path) {
+    1206             : 
+    1207           0 :   const std::string name_space = nh_.getNamespace() + "/";
+    1208             : 
+    1209           0 :   ROS_INFO("[%s]: loading '%s' under the namespace '%s'", getName().c_str(), file_path.c_str(), name_space.c_str());
+    1210             : 
+    1211             :   // load the user-requested file
+    1212             :   {
+    1213           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    1214           0 :     int         result  = std::system(command.c_str());
+    1215             : 
+    1216           0 :     if (result != 0) {
+    1217           0 :       ROS_ERROR("[%s]: failed to load '%s'", getName().c_str(), file_path.c_str());
+    1218           0 :       return false;
+    1219             :     }
+    1220             :   }
+    1221             : 
+    1222             :   // load the platform config
+    1223           0 :   if (_platform_config_ != "") {
+    1224           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    1225           0 :     int         result  = std::system(command.c_str());
+    1226             : 
+    1227           0 :     if (result != 0) {
+    1228           0 :       ROS_ERROR("[%s]: failed to load the platform config file '%s'", getName().c_str(), _platform_config_.c_str());
+    1229           0 :       return false;
+    1230             :     }
+    1231             :   }
+    1232             : 
+    1233             :   // load the custom config
+    1234           0 :   if (_custom_config_ != "") {
+    1235           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    1236           0 :     int         result  = std::system(command.c_str());
+    1237             : 
+    1238           0 :     if (result != 0) {
+    1239           0 :       ROS_ERROR("[%s]: failed to load the custom config file '%s'", getName().c_str(), _custom_config_.c_str());
+    1240           0 :       return false;
+    1241             :     }
+    1242             :   }
+    1243             : 
+    1244           0 :   return true;
+    1245             : }
+    1246             : 
+    1247             : //}
+    1248             : 
+    1249             : }  // namespace estimation_manager
+    1250             : 
+    1251             : }  // namespace mrs_uav_managers
+    1252             : 
+    1253             : #include <pluginlib/class_list_macros.h>
+    1254          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::estimation_manager::EstimationManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..bf623ddd8d --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html @@ -0,0 +1,334 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..60b59e7f9d0abe9be2ec6a41d69f9797ee777404 GIT binary patch literal 4359 zcmV+i5%}(jP)YvKVy8L+Wyg_&HACO~`FGEWR3 z&pNLZXXHdNil~^b&44zp%`{eYZNw;ceSPDHkdaTb97X;$Km=R@901@Y#tzdsScc`| zereZQ-yOQvq>TiAN8h_sLII!=qn*ZGI9@*LqMe|s`_NIETm>UzZmbP}*AMoMnQ;X` zJH`V4=HWMMiH*S2wH46ZwE=G}jb%{tENYx=avQ9?nA^n!Rv;Xg(5B<>>|V_C0^2;_ z#(6Zgn+5I3gIgL`+T@*K}REe1> zV`Ir>Ua`)M*{o(Qq#fh)`Fy#yg+J^2&w4zp?#;dJfO}hCdE1_#d5mwkr{3OxTLHua z+O-ELg|Kq8h$r#9NfaOAfg*YNuWe3_t9x&K1;CYvO~hyt&&9T49HHvUl04*?yb3UF z$qMGQN0E=0&UVARaoYAegc3291bF~j6wNUs8WPO4$*8mnJeOZd;L%YDPp%gDYb6wO z0Rg`Z|KgERy=Gx5q!-7N4kPnx1ulzQ7C4snM=u=fo9#6a+ZV3S33k^<2-1_vr{G z%*X-taWfhk&WWw;dMZ0-6>;20hcXhBau6#Ah`TNZ&VZU}DpAArC?g@_u`_K=3t6fVnd|t>3RpXl zM2rY)HAagVj!*AdUs?TkW7ir1xa$Y{-q0Y$tZNwo?fN|5lxwBt>qfXf$1~8L(!rDX zu9&7Ab|i1WfdRaitK=_e7c%3AAqVt)3eh>8Z$!}pBDFXO!s-Je zT5)Jq)YaG`*R4GAY=gRZ1+D^NpSXG1hoI%=QsGz4iJbg!1Q%(qI^zSH23U&`L`hlf z7jR8bp2f%>8ZcvwlTejq1)#O-!>0)D!+i)Y(C!A@hZxy<`xIcTGxp)@o6%5=H&$pY zjpoVKc*-|hMtYv|y#thYZv%n(Z#*~rFj$rR1Pd6zR4uyHD6XMGdkiTJ!)SLqKaM{iDDQb0nIIs$_Lv>=Zoz^S8j=8I&pr+o zxZI$AWd1yREC$xI=luJx^3DD{`#;aVU3&yA`VpS}|8wO!BOX>l^6n!GA%nXT7)45k zH0PR^6b3xL%lqtg_2COyHF$w%{qUfbQCCZjCo75v{I_+DFk%^MTw`IM?0SC{5$ZIe zVb+PPhkxEo+)I*=5RsT3^$Mr4d6Kt z6q74VNx#RpL>1JGp%ztu8N&`yg|FBvToqM_Cq&Yjz^Di)sKfi=N278K@=w%p(rmd# z&tX*n$3m?T=o+&#yT=K>^m0r+>lkeX1J7N3()M%Tm7n(eR zG_yR3`beE}D4t0>Am8M2iN|a`MgzLBPT+gR`4G#kl!DXyBa~sOgCs|xW72J)xf(W;)Yoi=HT$f-^Kp=r{ z@FfsF;X348*5oRJ%ZMPtRl2#^E}nc~wpYBk3%II1s+!XB(I{{tbjF7B0W{6gqy;RW zb2L){^w)?dUxq&>9>u4~YLI?E*UUBbnLESNIOYz3-wucBjOh^A&AAk50c5gVhm%#t z=>0Y1Qvd|{$b8VH=v+Iaor%##4Zz{nc= zW4(pe02cVYLQRw3MS<-F_Uyq8zGPGN3bkA|K;ssGcnY9}h(Z+4;zKnh@xb$!q1N9Q z{6nQeaWm<7<=hAeF4$z&IC3-RVa8|2FtF`QReokL{*)$37Wff^qq)(sF6|3@rmqT@ ztC|6=azv9$iHc!4uE%dGfwI}v&$q3M96NcMKA5GM@6v)4S{>lEH=MFvPkAM6(-Jp4 z;$EAPlje~M9zdW95{N8?kw!`2*kI))TJgur$bM=*-!(0_6Uy=E(9A7dvs=uxWf5rP zFrMQ&uNtaSvu^Xc?n<2!#*qJ4FcaD?(Dqy5I6(eN^T&Uun$4IJiuD6dp zvZRbu#5ohWk$pK_X6p;Dbif0%aLf(?hRR3rxEt-d2B~XtN)&?5YN+5;C4Df0i8$GFoIS07J6A*|6scfOy zladYt=FUfU7|_4h!>4Fa3RWlQ#5G}NJ2LGr?<|Gi;qAgqk0Na!g9ArOK!82!VBZ~* z-2O~+sp}>U|1-_)H1;>mJ_yfo{b!n)Hbt&~k!fbPq)Y|s=TjP{Er2Ikwzs~vuo6(S z;23aLO+CzsNDo)__7}fON+&fjN#ly{wu3ORtKHs7} z<}83#?PdWyKI%M#Y9(o9(brC8cob;NQkp#XGftBDZ*yYmQklvS#N99;6&Hg$dcUYh zn>IQ!#zvN5`2fggf1U^b%o*Cpo&4#WhrY7whx~%d*A?!f;I#~RsM%V+&8qjE1%`tB z<30hMMgW+uEIuAtz;wpmhmBvs)lFF`_q@n;H}TA=(hAgIO((?T#8LqC$41s|3%dX> z&#)JO82x9Kt^*c=EfDref#3F`%X`cZ-v^(+4?jBwMkZ>lloj%`&^}$oi@)r`1)K8JP{&86;J(gmlk=cl|#;;g|qyyS8iZt4;r~)wndXdXU z4XMpaVL*T@SseseHWeLWDfLJ4$jvxNHo)7fTH>}XqRWl%XIHLZcl2os02dh0V-?rb zTuL*RNvE?c0*$(Xs_WXFVlA69yN}ypM|#hGGLrX7O&;me?{d{RJ=VA*N>(7=dinaC z>x!GS8rJTRYUfO9b1JrRf6kmb=a;#L9Fjk%op0d7UGY|oW{YbdLY_t@GTh}D!nKyd z+^#k#6|M2-Z}ZE+>VN*W;up2ij$ICO!D>d-NQ%gQUBV6g)^5CX)89krPbD93-k$MC zY_?74HFS-L6fXMXuKE)od$?RyeGImK-O^T_^?QuA{@M9bCpBFltgcyDzU8zuK`8+u zi+9}diL$96)-3*v4(v#KHs#cGV!XMq z#EDu{uM}OJ;h+!~9iiv7XH9zI4Yhq$h!qI_>#2@>Lt0mbF(TYz7UG3&8V$Fw+AxA6 z6~`(i50ly0KNE>zhr2r7*VR9HIj-Gg+Fz0iel(pb#N!steVHi%;u8NA9ss}*P1`=@ z!xd~{2=D4VdqNJyf*25|V1^J4 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:648872.7 %
Date:2024-04-25 05:42:57Functions:192382.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&, double)0
mrs_uav_managers::Estimator::getHeadingRate(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isStopped() const0
mrs_uav_managers::Estimator::changeState(mrs_uav_managers::estimation_manager::SMStates_t)2172
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)2172
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const2172
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const3234
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const4344
mrs_uav_managers::Estimator::getMaxFlightZ() const17566
mrs_uav_managers::Estimator::isStarted() const35148
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)283981
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)284281
mrs_uav_managers::Estimator::isReady() const339357
mrs_uav_managers::Estimator::getName[abi:cxx11]() const588351
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const695081
mrs_uav_managers::Estimator::isMitigatingJump()709274
mrs_uav_managers::Estimator::publishDiagnostics() const984589
mrs_uav_managers::Estimator::isError() const1283630
mrs_uav_managers::Estimator::isRunning() const1609302
mrs_uav_managers::Estimator::isInitialized() const2532428
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const7516483
mrs_uav_managers::Estimator::getCurrentSmState() const8247575
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html new file mode 100644 index 0000000000..f2c59bd6fa --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:648872.7 %
Date:2024-04-25 05:42:57Functions:192382.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::Estimator::changeState(mrs_uav_managers::estimation_manager::SMStates_t)2172
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)283981
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&, double)0
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)284281
mrs_uav_managers::Estimator::getHeadingRate(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_uav_managers::Estimator::isMitigatingJump()709274
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)2172
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const695081
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const3234
mrs_uav_managers::Estimator::getMaxFlightZ() const17566
mrs_uav_managers::Estimator::isInitialized() const2532428
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const4344
mrs_uav_managers::Estimator::getCurrentSmState() const8247575
mrs_uav_managers::Estimator::publishDiagnostics() const984589
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const2172
mrs_uav_managers::Estimator::getName[abi:cxx11]() const588351
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isError() const1283630
mrs_uav_managers::Estimator::isReady() const339357
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const7516483
mrs_uav_managers::Estimator::isRunning() const1609302
mrs_uav_managers::Estimator::isStarted() const35148
mrs_uav_managers::Estimator::isStopped() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.frameset.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.frameset.html new file mode 100644 index 0000000000..b138653798 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html new file mode 100644 index 0000000000..9a00fed780 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html @@ -0,0 +1,295 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:648872.7 %
Date:2024-04-25 05:42:57Functions:192382.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_managers/estimation_manager/estimator.h>
+       2             : 
+       3             : namespace mrs_uav_managers
+       4             : {
+       5             : 
+       6             : /*//{ method implementations */
+       7             : /*//{ changeState() */
+       8        2172 : bool Estimator::changeState(SMStates_t new_state) {
+       9             : 
+      10        2172 :   if (new_state == getCurrentSmState()) {
+      11           0 :     return true;
+      12             :   }
+      13             : 
+      14        2172 :   previous_sm_state_ = getCurrentSmState();
+      15        2172 :   setCurrentSmState(new_state);
+      16             : 
+      17        2172 :   ROS_INFO("[%s]: Switching sm state %s -> %s", getPrintName().c_str(), getSmStateString(previous_sm_state_).c_str(), getCurrentSmStateString().c_str());
+      18        2172 :   return true;
+      19             : }
+      20             : /*//}*/
+      21             : 
+      22             : /*//{ isInState() */
+      23     7516483 : bool Estimator::isInState(const SMStates_t& state_in) const {
+      24     7516483 :   return state_in == getCurrentSmState();
+      25             : }
+      26             : /*//}*/
+      27             : 
+      28             : /*//{ isInitialized() */
+      29     2532428 : bool Estimator::isInitialized() const {
+      30     2532428 :   return !isInState(UNINITIALIZED_STATE);
+      31             : }
+      32             : /*//}*/
+      33             : 
+      34             : /*//{ isReady() */
+      35      339357 : bool Estimator::isReady() const {
+      36      339357 :   return isInState(READY_STATE);
+      37             : }
+      38             : /*//}*/
+      39             : 
+      40             : /*//{ isStarted() */
+      41       35148 : bool Estimator::isStarted() const {
+      42       35148 :   return isInState(STARTED_STATE);
+      43             : }
+      44             : /*//}*/
+      45             : 
+      46             : /*//{ isRunning() */
+      47     1609302 : bool Estimator::isRunning() const {
+      48     1609302 :   return isInState(SMStates_t::RUNNING_STATE);
+      49             : }
+      50             : /*//}*/
+      51             : 
+      52             : /*//{ isStopped() */
+      53           0 : bool Estimator::isStopped() const {
+      54           0 :   return isInState(STOPPED_STATE);
+      55             : }
+      56             : /*//}*/
+      57             : 
+      58             : /*//{ isError() */
+      59     1283630 : bool Estimator::isError() const {
+      60     1283630 :   return isInState(ERROR_STATE);
+      61             : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ getCurrentSmState() */
+      65     8247575 : SMStates_t Estimator::getCurrentSmState() const {
+      66     8247575 :   return current_sm_state_;
+      67             : }
+      68             : /*//}*/
+      69             : 
+      70             : /*//{ setCurrentSmState() */
+      71        2172 : void Estimator::setCurrentSmState(const SMStates_t& new_state) {
+      72        2172 :   std::scoped_lock lock(mutex_current_state_);
+      73        2172 :   current_sm_state_ = new_state;
+      74        2172 : }
+      75             : /*//}*/
+      76             : 
+      77             : /*//{ getSmStateString() */
+      78        4344 : std::string Estimator::getSmStateString(const SMStates_t& state) const {
+      79        4344 :   return sm::state_names[state];
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ getCurrentSmStateName() */
+      84        2172 : std::string Estimator::getCurrentSmStateString(void) const {
+      85        4344 :   return getSmStateString(getCurrentSmState());
+      86             : }
+      87             : /*//}*/
+      88             : 
+      89             : /*//{ isMitigatingJump() */
+      90      709274 : bool Estimator::isMitigatingJump(void) {
+      91      709274 :   if (is_mitigating_jump_) {
+      92           6 :     is_mitigating_jump_ = false;
+      93           0 :     return true;
+      94             :   } else {
+      95      709271 :     return false;
+      96             :   }
+      97             : }
+      98             : /*//}*/
+      99             : 
+     100             : /*//{ getName() */
+     101      588351 : std::string Estimator::getName(void) const {
+     102      588351 :   return name_;
+     103             : }
+     104             : /*//}*/
+     105             : 
+     106             : /*//{ getPrintName() */
+     107        3234 : std::string Estimator::getPrintName(void) const {
+     108        6468 :   return ch_->nodelet_name + "/" + name_;
+     109             : }
+     110             : /*//}*/
+     111             : 
+     112             : /*//{ getType() */
+     113           0 : std::string Estimator::getType(void) const {
+     114           0 :   return type_;
+     115             : }
+     116             : /*//}*/
+     117             : 
+     118             : /*//{ getFrameId() */
+     119      695081 : std::string Estimator::getFrameId(void) const {
+     120      695081 :   return ns_frame_id_;
+     121             : }
+     122             : /*//}*/
+     123             : 
+     124             : /*//{ getMaxFlightZ() */
+     125       17566 : double Estimator::getMaxFlightZ(void) const {
+     126       17566 :   return max_flight_z_;
+     127             : }
+     128             : /*//}*/
+     129             : 
+     130             : /*//{ publishDiagnostics() */
+     131      984589 : void Estimator::publishDiagnostics() const {
+     132             : 
+     133      984589 :   if (!ch_->debug_topics.diag) {
+     134      984513 :     return;
+     135             :   }
+     136             : 
+     137           4 :   mrs_msgs::EstimatorDiagnostics msg;
+     138           0 :   msg.header.stamp       = ros::Time::now();
+     139           0 :   msg.header.frame_id    = getFrameId();
+     140           0 :   msg.estimator_name     = getName();
+     141           0 :   msg.estimator_type     = getType();
+     142           0 :   msg.estimator_sm_state = getCurrentSmStateString();
+     143             : 
+     144           0 :   ph_diagnostics_.publish(msg);
+     145             : }
+     146             : /*//}*/
+     147             : 
+     148             : /*//{ getAccGlobal() */
+     149           0 : tf2::Vector3 Estimator::getAccGlobal(const sensor_msgs::Imu::ConstPtr& input_msg, const double hdg) {
+     150             : 
+     151           0 :   geometry_msgs::Vector3Stamped acc_stamped;
+     152           0 :   acc_stamped.vector = input_msg->linear_acceleration;
+     153           0 :   acc_stamped.header = input_msg->header;
+     154           0 :   return getAccGlobal(acc_stamped, hdg);
+     155             : }
+     156             : 
+     157      284281 : tf2::Vector3 Estimator::getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr& input_msg, const double hdg) {
+     158             : 
+     159      566300 :   geometry_msgs::Vector3Stamped acc_stamped;
+     160      283781 :   acc_stamped.vector = input_msg->control_acceleration;
+     161      283669 :   acc_stamped.header = input_msg->header;
+     162      561980 :   return getAccGlobal(acc_stamped, hdg);
+     163             : }
+     164             : 
+     165      283981 : tf2::Vector3 Estimator::getAccGlobal(const geometry_msgs::Vector3Stamped& acc_stamped, const double hdg) {
+     166             : 
+     167             :   // untilt the desired acceleration vector
+     168      565174 :   geometry_msgs::Vector3Stamped des_acc;
+     169      283624 :   geometry_msgs::Vector3        des_acc_untilted;
+     170      283886 :   des_acc.vector.x        = acc_stamped.vector.x;
+     171      283886 :   des_acc.vector.y        = acc_stamped.vector.y;
+     172      283886 :   des_acc.vector.z        = acc_stamped.vector.z;
+     173      283886 :   des_acc.header.frame_id = ch_->frames.ns_fcu;
+     174      283969 :   des_acc.header.stamp    = acc_stamped.header.stamp;
+     175      567790 :   auto response_acc       = ch_->transformer->transformSingle(des_acc, ch_->frames.ns_fcu_untilted);
+     176      284316 :   if (response_acc) {
+     177      284317 :     des_acc_untilted.x = response_acc.value().vector.x;
+     178      284317 :     des_acc_untilted.y = response_acc.value().vector.y;
+     179      284317 :     des_acc_untilted.z = response_acc.value().vector.z;
+     180             :   } else {
+     181           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform from %s to %s failed", getPrintName().c_str(), des_acc.header.frame_id.c_str(),
+     182             :                       ch_->frames.ns_fcu_untilted.c_str());
+     183             :   }
+     184             : 
+     185             :   // rotate the desired acceleration vector to global frame
+     186      284317 :   const tf2::Vector3 des_acc_global = Support::rotateVecByHdg(des_acc_untilted, hdg);
+     187             : 
+     188      562239 :   return des_acc_global;
+     189             : }
+     190             : /*//}*/
+     191             : 
+     192             : /*//{ getHeadingRate() */
+     193           0 : std::optional<double> Estimator::getHeadingRate(const nav_msgs::OdometryConstPtr& odom_msg) {
+     194             : 
+     195             :   double hdg_rate;
+     196             :   try {
+     197           0 :     hdg_rate = mrs_lib::AttitudeConverter(odom_msg->pose.pose.orientation).getHeadingRate(odom_msg->twist.twist.angular);
+     198             :   }
+     199           0 :   catch (...) {
+     200           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading rate", getPrintName().c_str());
+     201           0 :     return {};
+     202             :   }
+     203           0 :   return hdg_rate;
+     204             : }
+     205             : /*//}*/
+     206             : 
+     207             : 
+     208             : /*//}*/
+     209             : 
+     210             : }  // namespace mrs_uav_managers
+     211             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html new file mode 100644 index 0000000000..c6ae1bd90d --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html @@ -0,0 +1,73 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1c2f244964b56738e6ebab53f813ef1318a68431 GIT binary patch literal 828 zcmV-C1H=4@P)4x!Q9c0@60NQ5w3(fqar+v0`4Uv`OSX>U_4Hn$E%7)8r`>ntGKTnxc!yaNN%bGx^R%&NFpK3xF{>b%s>;s@CdB) z@adE_%e!WcW&lFak0PeyoycJ1rG$(rE)5v=#N6=c~yM1xYk z!zH0Qm$1Xx|Gd7JHSh18K@qta`Z5KZ8Ysq{qIhp0Go4op9&G@@+NK`eGlF_+FvUeTrLtrC$ z`w)w+IO9v~c^zi0d8P-i1l(#s0HS%FfI%Z(60QKZ8k7x)O97PD9;lIA*LG6PxN;Wj zc2UO5r|aes1jSsezw$uLw)GtVaiEiXMl7*96vd)UrSrGfsodh71ixWNDliH}4&dz>(oMfm6H&K-~?2nZ^?9 zfMfg}9ZiqFrxr(ETLq1DaWbrj^Ufd_u8(1ILN)}jBe&Agt7U9QN z7U?>ck?0}JjpkgD3Wjb1`#9~jn_<`ota&^&IfQBbQk2UwU}?iQUV$6@CG*JNPl@5swZ40000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const73
mrs_uav_managers::AglEstimator::publishAglHeight() const139277
mrs_uav_managers::AglEstimator::publishCovariance() const139277
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html new file mode 100644 index 0000000000..bccda4603c --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::publishAglHeight() const139277
mrs_uav_managers::AglEstimator::publishCovariance() const139277
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const73
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.frameset.html b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.frameset.html new file mode 100644 index 0000000000..66ffffc405 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html new file mode 100644 index 0000000000..ea5a19930a --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_managers/agl_estimator.h>
+       2             : 
+       3             : namespace mrs_uav_managers
+       4             : {
+       5             : 
+       6             : /*//{ publishAglHeight() */
+       7      139277 : void AglEstimator::publishAglHeight() const {
+       8      139277 :   ph_agl_height_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_));
+       9      139277 : }
+      10             : /*//}*/
+      11             : 
+      12             : /*//{ publishCovariance() */
+      13      139277 : void AglEstimator::publishCovariance() const {
+      14             : 
+      15      139277 :   if (!ch_->debug_topics.covariance) {
+      16      139277 :     return;
+      17             :   }
+      18             : 
+      19           0 :   ph_agl_height_cov_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_cov_));
+      20             : }
+      21             : /*//}*/
+      22             : 
+      23             : /*//{ isCompatibleWithHwApi() */
+      24          73 : bool AglEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+      25             : 
+      26          73 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      27          73 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      28             : 
+      29          73 :   ph_->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      30             : 
+      31             :   bool requires_gnss, requires_imu, requires_distance_sensor, requires_altitude, requires_magnetometer_heading, requires_position, requires_orientation,
+      32             :       requires_velocity, requires_angular_velocity;
+      33             : 
+      34          73 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+      35          73 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+      36          73 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+      37          73 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+      38          73 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+      39          73 :   ph_->param_loader->loadParam("requires/position", requires_position);
+      40          73 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+      41          73 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+      42          73 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+      43             : 
+      44          73 :   if (!ph_->param_loader->loadedSuccessfully()) {
+      45           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      46           0 :     ros::shutdown();
+      47             :   }
+      48             : 
+      49          73 :   if (requires_gnss && !hw_api_capabilities->produces_gnss) {
+      50           0 :     ROS_ERROR("[%s]: requires gnss but hw api does not provide it.", getPrintName().c_str());
+      51           0 :     return false;
+      52             :   }
+      53             : 
+      54          73 :   if (requires_imu && !hw_api_capabilities->produces_imu) {
+      55           0 :     ROS_ERROR("[%s]: requires imu but hw api does not provide it.", getPrintName().c_str());
+      56           0 :     return false;
+      57             :   }
+      58             : 
+      59          73 :   if (requires_distance_sensor && !hw_api_capabilities->produces_distance_sensor) {
+      60           0 :     ROS_ERROR("[%s]: requires distance_sensor but hw api does not provide it.", getPrintName().c_str());
+      61           0 :     return false;
+      62             :   }
+      63             : 
+      64          73 :   if (requires_altitude && !hw_api_capabilities->produces_altitude) {
+      65           0 :     ROS_ERROR("[%s]: requires altitude but hw api does not provide it.", getPrintName().c_str());
+      66           0 :     return false;
+      67             :   }
+      68             : 
+      69          73 :   if (requires_magnetometer_heading && !hw_api_capabilities->produces_magnetometer_heading) {
+      70           0 :     ROS_ERROR("[%s]: requires magnetometer_heading but hw api does not provide it.", getPrintName().c_str());
+      71           0 :     return false;
+      72             :   }
+      73             : 
+      74          73 :   if (requires_position && !hw_api_capabilities->produces_position) {
+      75           0 :     ROS_ERROR("[%s]: requires position but hw api does not provide it.", getPrintName().c_str());
+      76           0 :     return false;
+      77             :   }
+      78             : 
+      79          73 :   if (requires_orientation && !hw_api_capabilities->produces_orientation) {
+      80           0 :     ROS_ERROR("[%s]: requires orientation but hw api does not provide it.", getPrintName().c_str());
+      81           0 :     return false;
+      82             :   }
+      83             : 
+      84          73 :   if (requires_velocity && !hw_api_capabilities->produces_velocity) {
+      85           0 :     ROS_ERROR("[%s]: requires velocity but hw api does not provide it.", getPrintName().c_str());
+      86           0 :     return false;
+      87             :   }
+      88             : 
+      89          73 :   if (requires_angular_velocity && !hw_api_capabilities->produces_angular_velocity) {
+      90           0 :     ROS_ERROR("[%s]: requires angular_velocity but hw api does not provide it.", getPrintName().c_str());
+      91           0 :     return false;
+      92             :   }
+      93             : 
+      94          73 :   return true;
+      95             : }
+      96             : /*//}*/
+      97             : 
+      98             : }  // namespace mrs_uav_managers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.overview.html new file mode 100644 index 0000000000..0cae5e00e5 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.overview.html @@ -0,0 +1,45 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8e2a94c7d8572c5b8eb0b9ee2d0f2e729546678b GIT binary patch literal 454 zcmV;%0XhDOP)4mx~dDAPk1%2Dk!j5LciZ=>Dq|R1i&o_~?~0cw)UY@bQP7K<}*U52oOT%>hEB zlH;vWGDF9K`>qsz7|gIYbQkKQfgXysMnMhqzr4ZhE^)j>tx79Ssr?JXP)lJniAbZZ z5oO$A5H3CyY$|*yU8i9G4GMOFYd01t*BG3rT%aqCP wnEak0K>vlAKS%1H3gT1T^G5*HDbdOK3oNIBVw!Iu5C8xG07*qoM6N<$g5h<@b^rhX literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html new file mode 100644 index 0000000000..9ff10306da --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-25 05:42:57Functions:1313100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.cpp +
58.8%58.8%
+ 		58.8 %30 / 51100.0 %3 / 3
<unnamed>58.8 %30 / 51100.0 %3 / 3
state_estimator.cpp +
61.9%61.9%
+ 		61.9 %52 / 84100.0 %10 / 10
<unnamed>61.9 %52 / 84100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html new file mode 100644 index 0000000000..c7f5005c2a --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-25 05:42:57Functions:1313100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.cpp +
58.8%58.8%
+ 		58.8 %30 / 51100.0 %3 / 3
<unnamed>58.8 %30 / 51100.0 %3 / 3
state_estimator.cpp +
61.9%61.9%
+ 		61.9 %52 / 84100.0 %10 / 10
<unnamed>61.9 %52 / 84100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/index-detail.html b/mrs_uav_managers/src/estimation_manager/estimators/index-detail.html new file mode 100644 index 0000000000..b76d9347e7 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-25 05:42:57Functions:1313100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.cpp +
58.8%58.8%
+ 		58.8 %30 / 51100.0 %3 / 3
<unnamed>58.8 %30 / 51100.0 %3 / 3
state_estimator.cpp +
61.9%61.9%
+ 		61.9 %52 / 84100.0 %10 / 10
<unnamed>61.9 %52 / 84100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html new file mode 100644 index 0000000000..77d2600b08 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-25 05:42:57Functions:1313100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.cpp +
58.8%58.8%
+ 		58.8 %30 / 51100.0 %3 / 3
state_estimator.cpp +
61.9%61.9%
+ 		61.9 %52 / 84100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html new file mode 100644 index 0000000000..65fa319c21 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-25 05:42:57Functions:1313100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.cpp +
58.8%58.8%
+ 		58.8 %30 / 51100.0 %3 / 3
state_estimator.cpp +
61.9%61.9%
+ 		61.9 %52 / 84100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/index.html b/mrs_uav_managers/src/estimation_manager/estimators/index.html new file mode 100644 index 0000000000..8ecd51f72f --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-25 05:42:57Functions:1313100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
agl_estimator.cpp +
58.8%58.8%
+ 		58.8 %30 / 51100.0 %3 / 3
state_estimator.cpp +
61.9%61.9%
+ 		61.9 %52 / 84100.0 %10 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html new file mode 100644 index 0000000000..0ffa490212 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const100
mrs_uav_managers::StateEstimator::getInnovation() const158630
mrs_uav_managers::StateEstimator::getPoseCovariance() const158630
mrs_uav_managers::StateEstimator::getTwistCovariance() const158630
mrs_uav_managers::StateEstimator::getUavState()175821
mrs_uav_managers::StateEstimator::publishOdom() const184615
mrs_uav_managers::StateEstimator::publishUavState() const184615
mrs_uav_managers::StateEstimator::publishCovariance() const184616
mrs_uav_managers::StateEstimator::publishInnovation() const184616
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const374160
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html new file mode 100644 index 0000000000..268a32ec08 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::getUavState()175821
mrs_uav_managers::StateEstimator::publishOdom() const184615
mrs_uav_managers::StateEstimator::getInnovation() const158630
mrs_uav_managers::StateEstimator::publishUavState() const184615
mrs_uav_managers::StateEstimator::getPoseCovariance() const158630
mrs_uav_managers::StateEstimator::publishCovariance() const184616
mrs_uav_managers::StateEstimator::publishInnovation() const184616
mrs_uav_managers::StateEstimator::getTwistCovariance() const158630
mrs_uav_managers::StateEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const100
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const374160
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.frameset.html b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.frameset.html new file mode 100644 index 0000000000..67d1a1c693 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html new file mode 100644 index 0000000000..3fb1e090ae --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html @@ -0,0 +1,263 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-04-25 05:42:57Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_managers/state_estimator.h>
+       2             : 
+       3             : namespace mrs_uav_managers
+       4             : {
+       5             : 
+       6             : 
+       7             : /*//{ getUavState() */
+       8      175821 : std::optional<mrs_msgs::UavState> StateEstimator::getUavState() {
+       9             : 
+      10      175821 :   if (!isRunning()) {
+      11           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: getUavState() was called while estimator is not running", getPrintName().c_str());
+      12           0 :     return {};
+      13             :   }
+      14             : 
+      15      351583 :   return mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      16             : }
+      17             : /*//}*/
+      18             : 
+      19             : /*//{ getInnovation() */
+      20      158630 : nav_msgs::Odometry StateEstimator::getInnovation() const {
+      21      158630 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_);
+      22             : }
+      23             : /*//}*/
+      24             : 
+      25             : /*//{ getPoseCovariance() */
+      26      158630 : std::vector<double> StateEstimator::getPoseCovariance() const {
+      27      158630 :   return mrs_lib::get_mutexed(mtx_covariance_, pose_covariance_.values);
+      28             : }
+      29             : /*//}*/
+      30             : 
+      31             : /*//{ getTwistCovariance() */
+      32      158630 : std::vector<double> StateEstimator::getTwistCovariance() const {
+      33      158630 :   return mrs_lib::get_mutexed(mtx_covariance_, twist_covariance_.values);
+      34             : }
+      35             : /*//}*/
+      36             : 
+      37             : /*//{ publishUavState() */
+      38      184615 : void StateEstimator::publishUavState() const {
+      39             : 
+      40      184615 :   if (!ch_->debug_topics.state) {
+      41           0 :     return;
+      42             :   }
+      43             : 
+      44      369209 :   auto uav_state = mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      45      184604 :   ph_uav_state_.publish(uav_state);
+      46             : }
+      47             : /*//}*/
+      48             : 
+      49             : /*//{ publishOdom() */
+      50      184615 : void StateEstimator::publishOdom() const {
+      51             : 
+      52      369231 :   auto odom = mrs_lib::get_mutexed(mtx_odom_, odom_);
+      53      184616 :   ph_odom_.publish(odom);
+      54      184616 : }
+      55             : /*//}*/
+      56             : 
+      57             : /*//{ publishCovariance() */
+      58      184616 : void StateEstimator::publishCovariance() const {
+      59             : 
+      60      184616 :   if (!ch_->debug_topics.covariance) {
+      61      184616 :     return;
+      62             :   }
+      63             : 
+      64           0 :   auto pose_cov  = mrs_lib::get_mutexed(mtx_covariance_, pose_covariance_);
+      65           0 :   auto twist_cov = mrs_lib::get_mutexed(mtx_covariance_, twist_covariance_);
+      66           0 :   ph_pose_covariance_.publish(pose_cov);
+      67           0 :   ph_twist_covariance_.publish(twist_cov);
+      68             : }
+      69             : /*//}*/
+      70             : 
+      71             : /*//{ publishInnovation() */
+      72      184616 : void StateEstimator::publishInnovation() const {
+      73             : 
+      74      184616 :   if (!ch_->debug_topics.innovation) {
+      75      184616 :     return;
+      76             :   }
+      77             : 
+      78           0 :   auto innovation = mrs_lib::get_mutexed(mtx_innovation_, innovation_);
+      79           0 :   ph_innovation_.publish(innovation);
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ rotateQuaternionByHeading() */
+      84      374160 : std::optional<geometry_msgs::Quaternion> StateEstimator::rotateQuaternionByHeading(const geometry_msgs::Quaternion& q, const double hdg) const {
+      85             : 
+      86             :   try {
+      87      374160 :     tf2::Quaternion tf2_q = mrs_lib::AttitudeConverter(q);
+      88             : 
+      89             :     // Obtain heading from quaternion
+      90      373680 :     double q_hdg = 0;
+      91      373680 :     q_hdg        = mrs_lib::AttitudeConverter(q).getHeading();
+      92             : 
+      93             :     // Build rotation matrix from difference between new heading and quaternion heading
+      94      371892 :     tf2::Matrix3x3 rot_mat = mrs_lib::AttitudeConverter(Eigen::AngleAxisd(hdg - q_hdg, Eigen::Vector3d::UnitZ()));
+      95             : 
+      96             :     // Transform the quaternion orientation by the rotation matrix
+      97      372207 :     geometry_msgs::Quaternion q_new = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_q);
+      98      370807 :     return q_new;
+      99             :   }
+     100           0 :   catch (...) {
+     101           0 :     ROS_WARN("[rotateQuaternionByHeading()]: failed to rotate quaternion by heading");
+     102           0 :     return {};
+     103             :   }
+     104             : }
+     105             : /*//}*/
+     106             : 
+     107             : /*//{ isCompatibleWithHwApi() */
+     108         100 : bool StateEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+     109             : 
+     110         100 :   ph_->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+     111             : 
+     112             :   bool requires_gnss, requires_imu, requires_distance_sensor, requires_altitude, requires_magnetometer_heading, requires_position, requires_orientation,
+     113             :       requires_velocity, requires_angular_velocity;
+     114             : 
+     115         100 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+     116         100 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+     117         100 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+     118         100 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+     119         100 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+     120         100 :   ph_->param_loader->loadParam("requires/position", requires_position);
+     121         100 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+     122         100 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+     123         100 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+     124             : 
+     125         100 :   if (!ph_->param_loader->loadedSuccessfully()) {
+     126           0 :     ROS_ERROR("[%s]: Could not load all non-optional hw_api compatibility parameters. Shutting down.", getPrintName().c_str());
+     127           0 :     ros::shutdown();
+     128             :   }
+     129             : 
+     130         100 :   if (requires_gnss && !hw_api_capabilities->produces_gnss) {
+     131           0 :     ROS_ERROR("[%s]: requires gnss but hw api does not provide it.", getPrintName().c_str());
+     132           0 :     return false;
+     133             :   }
+     134             : 
+     135         100 :   if (requires_imu && !hw_api_capabilities->produces_imu) {
+     136           0 :     ROS_ERROR("[%s]: requires imu but hw api does not provide it.", getPrintName().c_str());
+     137           0 :     return false;
+     138             :   }
+     139             : 
+     140         100 :   if (requires_distance_sensor && !hw_api_capabilities->produces_distance_sensor) {
+     141           0 :     ROS_ERROR("[%s]: requires distance_sensor but hw api does not provide it.", getPrintName().c_str());
+     142           0 :     return false;
+     143             :   }
+     144             : 
+     145         100 :   if (requires_altitude && !hw_api_capabilities->produces_altitude) {
+     146           0 :     ROS_ERROR("[%s]: requires altitude but hw api does not provide it.", getPrintName().c_str());
+     147           0 :     return false;
+     148             :   }
+     149             : 
+     150         100 :   if (requires_magnetometer_heading && !hw_api_capabilities->produces_magnetometer_heading) {
+     151           0 :     ROS_ERROR("[%s]: requires magnetometer_heading but hw api does not provide it.", getPrintName().c_str());
+     152           0 :     return false;
+     153             :   }
+     154             : 
+     155         100 :   if (requires_position && !hw_api_capabilities->produces_position) {
+     156           0 :     ROS_ERROR("[%s]: requires position but hw api does not provide it.", getPrintName().c_str());
+     157           0 :     return false;
+     158             :   }
+     159             : 
+     160         100 :   if (requires_orientation && !hw_api_capabilities->produces_orientation) {
+     161           0 :     ROS_ERROR("[%s]: requires orientation but hw api does not provide it.", getPrintName().c_str());
+     162           0 :     return false;
+     163             :   }
+     164             : 
+     165         100 :   if (requires_velocity && !hw_api_capabilities->produces_velocity) {
+     166           0 :     ROS_ERROR("[%s]: requires velocity but hw api does not provide it.", getPrintName().c_str());
+     167           0 :     return false;
+     168             :   }
+     169             : 
+     170         100 :   if (requires_angular_velocity && !hw_api_capabilities->produces_angular_velocity) {
+     171           0 :     ROS_ERROR("[%s]: requires angular_velocity but hw api does not provide it.", getPrintName().c_str());
+     172           0 :     return false;
+     173             :   }
+     174             : 
+     175         100 :   return true;
+     176             : }
+     177             : /*//}*/
+     178             : 
+     179             : }  // namespace mrs_uav_managers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.overview.html new file mode 100644 index 0000000000..f56ba99c77 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.overview.html @@ -0,0 +1,65 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..bc72d319242788406c563dbf7102614ba4896b70 GIT binary patch literal 753 zcmV=vK{+=A%<`fugT|4D%D%Y?bpOdcEsv|33Z$bJdkYuU(ivHuSf z^%UPM@=~IOTh~HDj!Hx|6wC=>lS0_RtN$X3=c$vZ>m}V+X}&1;=z5b*QR;If{2X#UB6l$>!>VAj=P^#DtsAs{j$`hCuT#=fnKc6X5)<{06RF>=VX9Ua&T#t9I z3NFa>J+7}gM4d3A@l)Z)o~!2lIG89Y-uL6UKDJ>4x$<$&9563-#yQ-CO*(SkJ*PWv zC@FO#@sTp=>c?G0?Gzh<3jm-9X_Co8aejAq{m0Rxm1>z(F?K+%VNzG&y32P%^Z_0n zj(M)hzRiJQq2$^lTeV&p6w_Rrws3R39PRXk%%6}rAM`Uu(Wn61aAD#O%sppB> zS9-q;kM1!`BkLQ--Ca5@;Uj}aYZ_)ydSXyG`$ow)pRT8C6Njl&cA9zhR=(9VqwC`X zo#u3|embu^RUFRZ_UWyY6g(p@qw`Ls2)8&}@lg-FEBBZwN$05_5ilXl%LWp6V~T3j zJ+;?y#gt=WJY%N*D>G&mijLwf8?&PE=BVK2stZU~0HnKLQem5xX~Wi`0H8;=Ck5a} zn1w)rZkz-H#+WTl@lyWc%pue|RkLAN`bjVxxOEX2GnN*W5u_qJOMpQk`M_avOfayWPm}wy`0_Kk~gJg`s12O{@fknXlF=o(=!E>eo^j$FaI5Ive jeDOweqg!!C#q@pv02dn3iqW*`00000NkvXXu0mjf+MZSW literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html b/mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..73d4c008f1 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-04-25 05:42:57Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimator.cpp +
72.7%72.7%
+ 		72.7 %64 / 8882.6 %19 / 23
<unnamed>72.7 %64 / 8882.6 %19 / 23
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %373 / 53787.5 %21 / 24
<unnamed>69.5 %373 / 53787.5 %21 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..8de114a6b1 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-04-25 05:42:57Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %373 / 53787.5 %21 / 24
<unnamed>69.5 %373 / 53787.5 %21 / 24
estimator.cpp +
72.7%72.7%
+ 		72.7 %64 / 8882.6 %19 / 23
<unnamed>72.7 %64 / 8882.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail.html b/mrs_uav_managers/src/estimation_manager/index-detail.html new file mode 100644 index 0000000000..239cfcf2b1 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-04-25 05:42:57Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %373 / 53787.5 %21 / 24
<unnamed>69.5 %373 / 53787.5 %21 / 24
estimator.cpp +
72.7%72.7%
+ 		72.7 %64 / 8882.6 %19 / 23
<unnamed>72.7 %64 / 8882.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-sort-f.html b/mrs_uav_managers/src/estimation_manager/index-sort-f.html new file mode 100644 index 0000000000..e5a2a1b3ec --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-04-25 05:42:57Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimator.cpp +
72.7%72.7%
+ 		72.7 %64 / 8882.6 %19 / 23
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %373 / 53787.5 %21 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-sort-l.html new file mode 100644 index 0000000000..9b45e2f21b --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-04-25 05:42:57Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %373 / 53787.5 %21 / 24
estimator.cpp +
72.7%72.7%
+ 		72.7 %64 / 8882.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index.html b/mrs_uav_managers/src/estimation_manager/index.html new file mode 100644 index 0000000000..fafbfc5c0c --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-04-25 05:42:57Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %373 / 53787.5 %21 / 24
estimator.cpp +
72.7%72.7%
+ 		72.7 %64 / 8882.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..816342736a --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22425886.8 %
Date:2024-04-25 05:42:57Functions:77100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::gain_manager::GainManager::callbackSetGains(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)2
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::gain_manager::GainManager::onInit()94
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)95
mrs_uav_managers::gain_manager::GainManager::stringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)2076
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)2154
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)21979
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.func.html b/mrs_uav_managers/src/gain_manager.cpp.func.html new file mode 100644 index 0000000000..db134a54b7 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.func.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22425886.8 %
Date:2024-04-25 05:42:57Functions:77100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::gain_manager::GainManager::stringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)2076
mrs_uav_managers::gain_manager::GainManager::callbackSetGains(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)2
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)2154
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)21979
mrs_uav_managers::gain_manager::GainManager::onInit()94
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)95
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/gain_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..dbde2f3cff --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.gcov.html b/mrs_uav_managers/src/gain_manager.cpp.gcov.html new file mode 100644 index 0000000000..1ee7c46c03 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.html @@ -0,0 +1,730 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22425886.8 %
Date:2024-04-25 05:42:57Functions:77100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <nodelet/nodelet.h>
+       5             : 
+       6             : #include <std_msgs/String.h>
+       7             : 
+       8             : #include <mrs_msgs/String.h>
+       9             : #include <mrs_msgs/EstimationDiagnostics.h>
+      10             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      11             : #include <mrs_msgs/GainManagerDiagnostics.h>
+      12             : 
+      13             : #include <mrs_lib/profiler.h>
+      14             : #include <mrs_lib/scope_timer.h>
+      15             : #include <mrs_lib/param_loader.h>
+      16             : #include <mrs_lib/mutex.h>
+      17             : #include <mrs_lib/publisher_handler.h>
+      18             : #include <mrs_lib/service_client_handler.h>
+      19             : #include <mrs_lib/subscribe_handler.h>
+      20             : 
+      21             : #include <dynamic_reconfigure/ReconfigureRequest.h>
+      22             : #include <dynamic_reconfigure/Reconfigure.h>
+      23             : #include <dynamic_reconfigure/Config.h>
+      24             : 
+      25             : //}
+      26             : 
+      27             : namespace mrs_uav_managers
+      28             : {
+      29             : 
+      30             : namespace gain_manager
+      31             : {
+      32             : 
+      33             : /* //{ class GainManager */
+      34             : 
+      35             : typedef struct
+      36             : {
+      37             : 
+      38             :   double kpxy, kiwxy, kibxy, kvxy, kaxy;
+      39             :   double kpz, kvz, kaz;
+      40             :   double kiwxy_lim, kibxy_lim;
+      41             :   double km, km_lim;
+      42             :   double kqrp, kqy;
+      43             : 
+      44             :   std::string name;
+      45             : 
+      46             : } Gains_t;
+      47             : 
+      48             : class GainManager : public nodelet::Nodelet {
+      49             : 
+      50             : public:
+      51             :   virtual void onInit();
+      52             : 
+      53             : private:
+      54             :   ros::NodeHandle nh_;
+      55             : 
+      56             :   std::atomic<bool> is_initialized_ = false;
+      57             : 
+      58             :   // | ----------------------- parameters ----------------------- |
+      59             : 
+      60             :   std::vector<std::string> _current_state_estimators_;
+      61             : 
+      62             :   std::vector<std::string>       _gain_names_;
+      63             :   std::map<std::string, Gains_t> _gains_;
+      64             : 
+      65             :   std::map<std::string, std::vector<std::string>> _map_type_allowed_gains_;
+      66             :   std::map<std::string, std::string>              _map_type_default_gains_;
+      67             :   ;
+      68             : 
+      69             :   // | --------------------- service clients -------------------- |
+      70             : 
+      71             :   ros::ServiceClient service_client_set_gains_;
+      72             : 
+      73             :   // | ----------------------- subscribers ---------------------- |
+      74             : 
+      75             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>     sh_estimation_diag_;
+      76             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+      77             : 
+      78             :   // | --------------------- gain management -------------------- |
+      79             : 
+      80             :   bool setGains(std::string gains_name);
+      81             : 
+      82             :   ros::ServiceServer service_server_set_gains_;
+      83             :   bool               callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+      84             : 
+      85             :   std::string last_estimator_name_;
+      86             :   std::mutex  mutex_last_estimator_name_;
+      87             : 
+      88             :   void       timerGainManagement(const ros::TimerEvent& event);
+      89             :   ros::Timer timer_gain_management_;
+      90             :   double     _gain_management_rate_;
+      91             : 
+      92             :   std::string current_gains_;
+      93             :   std::mutex  mutex_current_gains_;
+      94             : 
+      95             :   // | ------------------ diagnostics publisher ----------------- |
+      96             : 
+      97             :   mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics> ph_diagnostics_;
+      98             : 
+      99             :   void       timerDiagnostics(const ros::TimerEvent& event);
+     100             :   ros::Timer timer_diagnostics_;
+     101             :   double     _diagnostics_rate_;
+     102             : 
+     103             :   // | ------------------------ profiler ------------------------ |
+     104             : 
+     105             :   mrs_lib::Profiler profiler_;
+     106             :   bool              _profiler_enabled_ = false;
+     107             : 
+     108             :   // | ------------------- scope timer logger ------------------- |
+     109             : 
+     110             :   bool                                       scope_timer_enabled_ = false;
+     111             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     112             : 
+     113             :   // | ------------------------- helpers ------------------------ |
+     114             : 
+     115             :   bool stringInVector(const std::string& value, const std::vector<std::string>& vector);
+     116             : };
+     117             : 
+     118             : //}
+     119             : 
+     120             : /* //{ onInit() */
+     121             : 
+     122          94 : void GainManager::onInit() {
+     123             : 
+     124         188 :   ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     125             : 
+     126          94 :   ros::Time::waitForValid();
+     127             : 
+     128          94 :   ROS_INFO("[GainManager]: initializing");
+     129             : 
+     130             :   // | ------------------------- params ------------------------- |
+     131             : 
+     132         188 :   mrs_lib::ParamLoader param_loader(nh_, "GainManager");
+     133             : 
+     134         188 :   std::string custom_config_path;
+     135         188 :   std::string platform_config_path;
+     136             : 
+     137          94 :   param_loader.loadParam("custom_config", custom_config_path);
+     138          94 :   param_loader.loadParam("platform_config", platform_config_path);
+     139             : 
+     140          94 :   if (custom_config_path != "") {
+     141          94 :     param_loader.addYamlFile(custom_config_path);
+     142             :   }
+     143             : 
+     144          94 :   if (platform_config_path != "") {
+     145          94 :     param_loader.addYamlFile(platform_config_path);
+     146             :   }
+     147             : 
+     148          94 :   param_loader.addYamlFileFromParam("private_config");
+     149          94 :   param_loader.addYamlFileFromParam("public_config");
+     150          94 :   param_loader.addYamlFileFromParam("public_gains");
+     151             : 
+     152         188 :   const std::string yaml_prefix = "mrs_uav_managers/gain_manager/";
+     153             : 
+     154             :   // params passed from the launch file are not prefixed
+     155          94 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     156             : 
+     157          94 :   param_loader.loadParam(yaml_prefix + "gains", _gain_names_);
+     158             : 
+     159          94 :   param_loader.loadParam(yaml_prefix + "estimator_types", _current_state_estimators_);
+     160             : 
+     161          94 :   param_loader.loadParam(yaml_prefix + "rate", _gain_management_rate_);
+     162          94 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     163             : 
+     164             :   // | ------------------- scope timer logger ------------------- |
+     165             : 
+     166          94 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     167         282 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     168          94 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     169             : 
+     170          94 :   std::vector<std::string>::iterator it;
+     171             : 
+     172             :   // loading gain_names
+     173         282 :   for (it = _gain_names_.begin(); it != _gain_names_.end(); ++it) {
+     174             : 
+     175         188 :     ROS_INFO_STREAM("[GainManager]: loading gains '" << *it << "'");
+     176             : 
+     177         188 :     Gains_t new_gains;
+     178             : 
+     179         188 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kp", new_gains.kpxy);
+     180         188 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kv", new_gains.kvxy);
+     181         188 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/ka", new_gains.kaxy);
+     182         188 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib", new_gains.kibxy);
+     183         188 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw", new_gains.kiwxy);
+     184         188 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib_lim", new_gains.kibxy_lim);
+     185         188 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw_lim", new_gains.kiwxy_lim);
+     186             : 
+     187         188 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kp", new_gains.kpz);
+     188         188 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kv", new_gains.kvz);
+     189         188 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ka", new_gains.kaz);
+     190             : 
+     191         188 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_roll_pitch", new_gains.kqrp);
+     192         188 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_yaw", new_gains.kqy);
+     193             : 
+     194         188 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km", new_gains.km);
+     195         188 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km_lim", new_gains.km_lim);
+     196             : 
+     197         188 :     _gains_.insert(std::pair<std::string, Gains_t>(*it, new_gains));
+     198             :   }
+     199             : 
+     200             :   // loading the allowed gains lists
+     201         752 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     202             : 
+     203         658 :     std::vector<std::string> temp_vector;
+     204         658 :     param_loader.loadParam(yaml_prefix + "allowed_gains/" + *it, temp_vector);
+     205             : 
+     206         658 :     std::vector<std::string>::iterator it2;
+     207        1974 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     208        1316 :       if (!stringInVector(*it2, _gain_names_)) {
+     209           0 :         ROS_ERROR("[GainManager]: the element '%s' of %s/allowed_gains is not a valid gain!", it2->c_str(), it->c_str());
+     210           0 :         ros::shutdown();
+     211             :       }
+     212             :     }
+     213             : 
+     214         658 :     _map_type_allowed_gains_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     215             :   }
+     216             : 
+     217             :   // loading the default gains
+     218         752 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     219             : 
+     220         658 :     std::string temp_str;
+     221         658 :     param_loader.loadParam(yaml_prefix + "default_gains/" + *it, temp_str);
+     222             : 
+     223         658 :     if (!stringInVector(temp_str, _map_type_allowed_gains_.at(*it))) {
+     224           0 :       ROS_ERROR("[GainManager]: the element '%s' of %s/allowed_gains is not a valid gain!", temp_str.c_str(), it->c_str());
+     225           0 :       ros::shutdown();
+     226             :     }
+     227             : 
+     228         658 :     _map_type_default_gains_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     229             :   }
+     230             : 
+     231          94 :   ROS_INFO("[GainManager]: done loading dynamical params");
+     232             : 
+     233          94 :   current_gains_       = "";
+     234          94 :   last_estimator_name_ = "";
+     235             : 
+     236             :   // | ------------------------ services ------------------------ |
+     237             : 
+     238          94 :   service_server_set_gains_ = nh_.advertiseService("set_gains_in", &GainManager::callbackSetGains, this);
+     239             : 
+     240          94 :   service_client_set_gains_ = nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_gains_out");
+     241             : 
+     242             :   // | ----------------------- subscribers ---------------------- |
+     243             : 
+     244         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+     245          94 :   shopts.nh                 = nh_;
+     246          94 :   shopts.node_name          = "GainManager";
+     247          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     248          94 :   shopts.threadsafe         = true;
+     249          94 :   shopts.autostart          = true;
+     250          94 :   shopts.queue_size         = 10;
+     251          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     252             : 
+     253          94 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     254          94 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     255             : 
+     256             :   // | ----------------------- publishers ----------------------- |
+     257             : 
+     258          94 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics>(nh_, "diagnostics_out", 10);
+     259             : 
+     260             :   // | ------------------------- timers ------------------------- |
+     261             : 
+     262          94 :   timer_gain_management_ = nh_.createTimer(ros::Rate(_gain_management_rate_), &GainManager::timerGainManagement, this);
+     263          94 :   timer_diagnostics_     = nh_.createTimer(ros::Rate(_diagnostics_rate_), &GainManager::timerDiagnostics, this);
+     264             : 
+     265             :   // | ------------------------ profiler ------------------------ |
+     266             : 
+     267          94 :   profiler_ = mrs_lib::Profiler(nh_, "GainManager", _profiler_enabled_);
+     268             : 
+     269             :   // | ----------------------- finish init ---------------------- |
+     270             : 
+     271          94 :   if (!param_loader.loadedSuccessfully()) {
+     272           0 :     ROS_ERROR("[GainManager]: could not load all parameters!");
+     273           0 :     ros::shutdown();
+     274             :   }
+     275             : 
+     276          94 :   is_initialized_ = true;
+     277             : 
+     278          94 :   ROS_INFO("[GainManager]: initialized");
+     279             : 
+     280          94 :   ROS_DEBUG("[GainManager]: debug output is enabled");
+     281          94 : }
+     282             : 
+     283             : //}
+     284             : 
+     285             : // --------------------------------------------------------------
+     286             : // |                           methods                          |
+     287             : // --------------------------------------------------------------
+     288             : 
+     289             : /* setGains() //{ */
+     290             : 
+     291          95 : bool GainManager::setGains(std::string gains_name) {
+     292             : 
+     293          95 :   std::map<std::string, Gains_t>::iterator it;
+     294          95 :   it = _gains_.find(gains_name);
+     295             : 
+     296          95 :   if (it == _gains_.end()) {
+     297           0 :     ROS_WARN("[GainManager]: can not set gains for '%s', the mode is not on a list!", gains_name.c_str());
+     298           0 :     return false;
+     299             :   }
+     300             : 
+     301         190 :   dynamic_reconfigure::Config          conf;
+     302         190 :   dynamic_reconfigure::DoubleParameter param;
+     303             : 
+     304          95 :   param.name  = "kpxy";
+     305          95 :   param.value = it->second.kpxy;
+     306          95 :   conf.doubles.push_back(param);
+     307             : 
+     308          95 :   param.name  = "kvxy";
+     309          95 :   param.value = it->second.kvxy;
+     310          95 :   conf.doubles.push_back(param);
+     311             : 
+     312          95 :   param.name  = "kaxy";
+     313          95 :   param.value = it->second.kaxy;
+     314          95 :   conf.doubles.push_back(param);
+     315             : 
+     316          95 :   param.name  = "kq_roll_pitch";
+     317          95 :   param.value = it->second.kqrp;
+     318          95 :   conf.doubles.push_back(param);
+     319             : 
+     320          95 :   param.name  = "kibxy";
+     321          95 :   param.value = it->second.kibxy;
+     322          95 :   conf.doubles.push_back(param);
+     323             : 
+     324          95 :   param.name  = "kiwxy";
+     325          95 :   param.value = it->second.kiwxy;
+     326          95 :   conf.doubles.push_back(param);
+     327             : 
+     328          95 :   param.name  = "kibxy_lim";
+     329          95 :   param.value = it->second.kibxy_lim;
+     330          95 :   conf.doubles.push_back(param);
+     331             : 
+     332          95 :   param.name  = "kiwxy_lim";
+     333          95 :   param.value = it->second.kiwxy_lim;
+     334          95 :   conf.doubles.push_back(param);
+     335             : 
+     336          95 :   param.name  = "kpz";
+     337          95 :   param.value = it->second.kpz;
+     338          95 :   conf.doubles.push_back(param);
+     339             : 
+     340          95 :   param.name  = "kvz";
+     341          95 :   param.value = it->second.kvz;
+     342          95 :   conf.doubles.push_back(param);
+     343             : 
+     344          95 :   param.name  = "kaz";
+     345          95 :   param.value = it->second.kaz;
+     346          95 :   conf.doubles.push_back(param);
+     347             : 
+     348          95 :   param.name  = "kq_yaw";
+     349          95 :   param.value = it->second.kqy;
+     350          95 :   conf.doubles.push_back(param);
+     351             : 
+     352          95 :   param.name  = "km";
+     353          95 :   param.value = it->second.km;
+     354          95 :   conf.doubles.push_back(param);
+     355             : 
+     356          95 :   param.name  = "km_lim";
+     357          95 :   param.value = it->second.km_lim;
+     358          95 :   conf.doubles.push_back(param);
+     359             : 
+     360         190 :   dynamic_reconfigure::ReconfigureRequest  srv_req;
+     361         190 :   dynamic_reconfigure::ReconfigureResponse srv_resp;
+     362             : 
+     363          95 :   srv_req.config = conf;
+     364             : 
+     365         190 :   dynamic_reconfigure::Reconfigure reconf;
+     366          95 :   reconf.request = srv_req;
+     367             : 
+     368          95 :   ROS_INFO_THROTTLE(1.0, "[GainManager]: setting up gains for '%s'", gains_name.c_str());
+     369             : 
+     370          95 :   bool res = service_client_set_gains_.call(reconf);
+     371             : 
+     372          95 :   if (!res) {
+     373             : 
+     374           0 :     ROS_WARN_THROTTLE(1.0, "[GainManager]: the service for setting gains has failed!");
+     375           0 :     return false;
+     376             : 
+     377             :   } else {
+     378             : 
+     379          95 :     mrs_lib::set_mutexed(mutex_current_gains_, gains_name, current_gains_);
+     380             : 
+     381          95 :     return true;
+     382             :   }
+     383             : }
+     384             : 
+     385             : //}
+     386             : 
+     387             : // --------------------------------------------------------------
+     388             : // |                          callbacks                         |
+     389             : // --------------------------------------------------------------
+     390             : 
+     391             : // | -------------------- service callbacks ------------------- |
+     392             : 
+     393             : /* //{ callbackSetGains() */
+     394             : 
+     395           2 : bool GainManager::callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+     396             : 
+     397           2 :   if (!is_initialized_) {
+     398           0 :     return false;
+     399             :   }
+     400             : 
+     401           4 :   std::stringstream ss;
+     402             : 
+     403           2 :   if (!sh_estimation_diag_.hasMsg()) {
+     404             : 
+     405           0 :     ss << "missing estimation diagnostics";
+     406             : 
+     407           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     408             : 
+     409           0 :     res.message = ss.str();
+     410           0 :     res.success = false;
+     411           0 :     return true;
+     412             :   }
+     413             : 
+     414           4 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     415             : 
+     416           2 :   if (!stringInVector(req.value, _gain_names_)) {
+     417             : 
+     418           1 :     ss << "the gains '" << req.value.c_str() << "' do not exist (in the GainManager's config)";
+     419             : 
+     420           1 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     421             : 
+     422           1 :     res.message = ss.str();
+     423           1 :     res.success = false;
+     424           1 :     return true;
+     425             :   }
+     426             : 
+     427           1 :   if (!stringInVector(req.value, _map_type_allowed_gains_.at(estimation_diagnostics->current_state_estimator))) {
+     428             : 
+     429           0 :     ss << "the gains '" << req.value.c_str() << "' are not allowed given the current state estimator";
+     430             : 
+     431           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     432             : 
+     433           0 :     res.message = ss.str();
+     434           0 :     res.success = false;
+     435           0 :     return true;
+     436             :   }
+     437             : 
+     438             :   // try to set the gains
+     439           1 :   if (!setGains(req.value)) {
+     440             : 
+     441           0 :     ss << "the Se3Controller could not set the gains";
+     442             : 
+     443           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     444             : 
+     445           0 :     res.message = ss.str();
+     446           0 :     res.success = false;
+     447           0 :     return true;
+     448             : 
+     449             :   } else {
+     450             : 
+     451           1 :     ss << "the gains '" << req.value.c_str() << "' are set";
+     452             : 
+     453           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     454             : 
+     455           1 :     res.message = ss.str();
+     456           1 :     res.success = true;
+     457           1 :     return true;
+     458             :   }
+     459             : }
+     460             : 
+     461             : //}
+     462             : 
+     463             : // --------------------------------------------------------------
+     464             : // |                           timers                           |
+     465             : // --------------------------------------------------------------
+     466             : 
+     467             : /* timerGainManagement() //{ */
+     468             : 
+     469       21979 : void GainManager::timerGainManagement(const ros::TimerEvent& event) {
+     470             : 
+     471       21979 :   if (!is_initialized_) {
+     472        4841 :     return;
+     473             :   }
+     474             : 
+     475       43958 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("gainManagementTimer", _gain_management_rate_, 0.01, event);
+     476       43958 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::gainManagementTimer", scope_timer_logger_, scope_timer_enabled_);
+     477             : 
+     478       21979 :   if (!sh_estimation_diag_.hasMsg()) {
+     479        4841 :     return;
+     480             :   }
+     481             : 
+     482       17138 :   if (!sh_control_manager_diag_.hasMsg()) {
+     483           0 :     return;
+     484             :   }
+     485             : 
+     486       34276 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     487             : 
+     488       34276 :   auto control_manager_diagnostics = sh_estimation_diag_.getMsg();
+     489             : 
+     490       34276 :   auto current_gains       = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     491       17138 :   auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_);
+     492             : 
+     493             :   // | --- automatically set _gains_ when currrent state estimator changes -- |
+     494       17138 :   if (estimation_diagnostics->current_state_estimator != last_estimator_name) {
+     495             : 
+     496          99 :     ROS_INFO_THROTTLE(1.0, "[GainManager]: the state estimator has changed! %s -> %s", last_estimator_name_.c_str(),
+     497             :                       estimation_diagnostics->current_state_estimator.c_str());
+     498             : 
+     499          99 :     std::map<std::string, std::string>::iterator it;
+     500          99 :     it = _map_type_default_gains_.find(estimation_diagnostics->current_state_estimator);
+     501             : 
+     502          99 :     if (it == _map_type_default_gains_.end()) {
+     503             : 
+     504           0 :       ROS_WARN_THROTTLE(1.0, "[GainManager]: the state estimator '%s' was not specified in the gain_manager's config!",
+     505             :                         estimation_diagnostics->current_state_estimator.c_str());
+     506             : 
+     507             :     } else {
+     508             : 
+     509             :       // if the current gains are within the allowed estimator types, do nothing
+     510          99 :       if (stringInVector(current_gains, _map_type_allowed_gains_.at(estimation_diagnostics->current_state_estimator))) {
+     511             : 
+     512           5 :         last_estimator_name = estimation_diagnostics->current_state_estimator;
+     513             : 
+     514             :         // else, try to set the default gains
+     515             :       } else {
+     516             : 
+     517          94 :         ROS_WARN_THROTTLE(1.0, "[GainManager]: the current gains '%s' are not within the allowed gains for '%s'", current_gains.c_str(),
+     518             :                           estimation_diagnostics->current_state_estimator.c_str());
+     519             : 
+     520          94 :         if (setGains(it->second)) {
+     521             : 
+     522          94 :           last_estimator_name = estimation_diagnostics->current_state_estimator;
+     523             : 
+     524          94 :           ROS_INFO_THROTTLE(1.0, "[GainManager]: gains set to default: '%s'", it->second.c_str());
+     525             : 
+     526             :         } else {
+     527             : 
+     528           0 :           ROS_WARN_THROTTLE(1.0, "[GainManager]: could not set gains!");
+     529             :         }
+     530             :       }
+     531             :     }
+     532             :   }
+     533             : 
+     534       17138 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     535             : }
+     536             : 
+     537             : //}
+     538             : 
+     539             : /* timerDiagnostics() //{ */
+     540             : 
+     541        2154 : void GainManager::timerDiagnostics(const ros::TimerEvent& event) {
+     542             : 
+     543        2154 :   if (!is_initialized_) {
+     544         470 :     return;
+     545             :   }
+     546             : 
+     547        4308 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     548        4308 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     549             : 
+     550        2154 :   if (!sh_estimation_diag_.hasMsg()) {
+     551         468 :     ROS_WARN_THROTTLE(10.0, "[GainManager]: can not do gain management, missing estimator diagnostics!");
+     552         468 :     return;
+     553             :   }
+     554             : 
+     555        1686 :   if (!sh_control_manager_diag_.hasMsg()) {
+     556           0 :     ROS_WARN_THROTTLE(10.0, "[GainManager]: can not do gain management, missing control manager diagnostics!");
+     557           0 :     return;
+     558             :   }
+     559             : 
+     560        1686 :   auto current_gains = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     561             : 
+     562        1686 :   if (current_gains == "") { // this could happend just before timerGainManagement() finishes
+     563           2 :     return;
+     564             :   }
+     565             : 
+     566        1684 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     567             : 
+     568        1684 :   mrs_msgs::GainManagerDiagnostics diagnostics;
+     569             : 
+     570        1684 :   diagnostics.stamp        = ros::Time::now();
+     571        1684 :   diagnostics.current_name = current_gains;
+     572        1684 :   diagnostics.loaded       = _gain_names_;
+     573             : 
+     574             :   // get the available gains
+     575             :   {
+     576        1684 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     577        1684 :     it = _map_type_allowed_gains_.find(estimation_diagnostics->current_state_estimator);
+     578             : 
+     579        1684 :     if (it == _map_type_allowed_gains_.end()) {
+     580           0 :       ROS_WARN_THROTTLE(1.0, "[GainManager]: the estimator name '%s' was not specified in the gain_manager's config!",
+     581             :                         estimation_diagnostics->current_state_estimator.c_str());
+     582             :     } else {
+     583        1684 :       diagnostics.available = it->second;
+     584             :     }
+     585             :   }
+     586             : 
+     587             :   // get the current gain values
+     588             :   {
+     589        1684 :     std::map<std::string, Gains_t>::iterator it;
+     590        1684 :     it = _gains_.find(current_gains);
+     591             : 
+     592        1684 :     if (it == _gains_.end()) {
+     593           0 :       ROS_ERROR("[GainManager]: current gains '%s' not found in the gain list!", current_gains.c_str());
+     594           0 :       return;
+     595             :     }
+     596             : 
+     597        1684 :     diagnostics.current_values.kpxy = it->second.kpxy;
+     598        1684 :     diagnostics.current_values.kvxy = it->second.kvxy;
+     599        1684 :     diagnostics.current_values.kaxy = it->second.kaxy;
+     600             : 
+     601        1684 :     diagnostics.current_values.kqrp = it->second.kqrp;
+     602             : 
+     603        1684 :     diagnostics.current_values.kibxy     = it->second.kibxy;
+     604        1684 :     diagnostics.current_values.kibxy_lim = it->second.kibxy_lim;
+     605             : 
+     606        1684 :     diagnostics.current_values.kiwxy     = it->second.kiwxy;
+     607        1684 :     diagnostics.current_values.kiwxy_lim = it->second.kiwxy_lim;
+     608             : 
+     609        1684 :     diagnostics.current_values.kpz = it->second.kpz;
+     610        1684 :     diagnostics.current_values.kvz = it->second.kvz;
+     611        1684 :     diagnostics.current_values.kaz = it->second.kaz;
+     612             : 
+     613        1684 :     diagnostics.current_values.kqy = it->second.kqy;
+     614             : 
+     615        1684 :     diagnostics.current_values.km     = it->second.km;
+     616        1684 :     diagnostics.current_values.km_lim = it->second.km_lim;
+     617             :   }
+     618             : 
+     619        1684 :   ph_diagnostics_.publish(diagnostics);
+     620             : }
+     621             : 
+     622             : //}
+     623             : 
+     624             : // --------------------------------------------------------------
+     625             : // |                          routines                          |
+     626             : // --------------------------------------------------------------
+     627             : 
+     628             : /* stringInVector() //{ */
+     629             : 
+     630        2076 : bool GainManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     631             : 
+     632        2076 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     633          95 :     return false;
+     634             :   } else {
+     635        1981 :     return true;
+     636             :   }
+     637             : }
+     638             : 
+     639             : //}
+     640             : 
+     641             : }  // namespace gain_manager
+     642             : 
+     643             : }  // namespace mrs_uav_managers
+     644             : 
+     645             : #include <pluginlib/class_list_macros.h>
+     646          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::gain_manager::GainManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..dedda4d668 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html @@ -0,0 +1,182 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/gain_manager.cpp.gcov.png b/mrs_uav_managers/src/gain_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..26b079aec6e9ef3a915fcb0329e31572342913a7 GIT binary patch literal 2153 zcmV-v2$uJWP)t0{{R3FUJ9e0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp3{6OWTxkr*wpmP*g=$ThY zd{9YyPOr8!bW?o4T}f(yZ{PduLX%R&-=IK*cLTh`i32gO;NQFo6a7ui*V4=&@kui) zS}>Bm0gPOX4rX-0l;RwNOD3M$cysa0dk$c@TYL_xlAeXEc)FyxBmxga^5`m4VUOr=J{!k zkV1~C@W|=cfe?8FAiwKq;p#F<5{UL)4t{x1BywPfuHh`CnIw74nY@KZvSnS9IL!qX zqh=1~;cdk|7Y?Y~_bf3TwSoW36idr+hX#{1r2tI;oTQkudQ4LU)@(&y?Ha)KpvM)7 zd#XGF0p(_ffFlq}!8HsxA#ij>?6h!~IBJL|UDEaCsep8J|H7V+MmtKz2VQ0|MA359 z0*>-%6s?LS`3rzr9O$7vKp8oP$mSNHkgol>+d%h9vTNljws};uU3-gh|9PNzOyPac z8eBNj_XIXo`r+uPt@yC<5dwGp>!9jMGsF76fI3-MoL7v>Q688?kX|Omes1}Aw-;#V z`*A(Lo~Qrl{pvgu;76rhz_SJjK>5r$84O2_I-|JTa4S6CWw_?+wQ=zbS9h+3g)huH zut27jy&$k|JxnY6khUImhxKHxM+bFgI_(b&A11nF0If09fZVR-lc|Y#uq0Cd>e`}s z^iq^R5G%}49Yjtlu@X&2x{;y>#y3pykU;Gyl6i`}>n6}qY~-XoQN*JLAhV^LvvN-c zj3UuW4an_U{?tBXU1{T<<54!FSrMa#0;d(DX9gqXq!MYX$XMWh!jZ1A?q!_EQg~Qu zamkl!yJA1z5BmXFhFQz?8^~L(d)DhPfM(po6gYMYKx_dy_4&dn?@#(yt zZNP5+{%TF{8yx_csHz>#1QnK3u2+R7>Zs0Ke{vS|+yKhz`j1Cp9ywiqIgborJC&# zDV`_68wzYwe75q!cNqmNe#lNhqMeKzg-;HbrNC^853|@Sv*yRsWmoBDf&kq)UGoJg z1Quz=xxccY6_O^vHbv1TuEQu$6&9^~UO!Fo!ZS?8(E8aHOfD8F!>1VWC5b)ELDs zN7@Q!q#fyR-YXOrDsMl5v~3mn4Hk5*3PDZWq~8$P+wyr1#rtaF;&epQ8?bE0^GX1j zt!E4Hh2rTc9UQA(_&A z+E=G}BmvT^()e<`A(Xl-a`Oo;f4ebU1~c_%rk3ZC*L`D^-<_HPWZEXZUq+4Qng%6%@w$ZmJ;6{M;Gu{c}-x%eRQFAo2bI6N`Y!>3RcxS6eg zPQYGd0L|m+-kaM~-y^cGgv?{QHi;Xb(K-BesL;z7PX+n?OlII2sd0)Iji{VP*B*#o zIz0dp4@A4>@@P*#eNb7GICoW`HWmI8kh1%-O+{s|U^fabwCLF@Fr%JFA|Fku`Wg&x z(%kmb#c>x*1LusQEVM_Af-00`w#+E#Li_S!th;UF_K@z{&C6emZ9DSMxEM>6INt)~ z3;b@12kuYNLLg{PKfCKd?h}CWJPuOS#O+u_;CLD4=lt9j+Q$#uob?^#?D@j*83#FH zx5>z>WG=$>e;wW-fnRCyi;s3cx8=?YTgI_mhF`fD27v$N3FNlI+CO=U5;XnM0c + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1265171873.6 %
Date:2024-04-25 05:42:57Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
<unnamed>86.8 %224 / 258100.0 %7 / 7
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
<unnamed>84.1 %195 / 232100.0 %8 / 8
uav_manager.cpp +
68.9%68.9%
+ 		68.9 %805 / 1169100.0 %38 / 38
<unnamed>68.9 %805 / 1169100.0 %38 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-detail-sort-l.html b/mrs_uav_managers/src/index-detail-sort-l.html new file mode 100644 index 0000000000..32358703f4 --- /dev/null +++ b/mrs_uav_managers/src/index-detail-sort-l.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1265171873.6 %
Date:2024-04-25 05:42:57Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
uav_manager.cpp +
68.9%68.9%
+ 		68.9 %805 / 1169100.0 %38 / 38
<unnamed>68.9 %805 / 1169100.0 %38 / 38
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
<unnamed>84.1 %195 / 232100.0 %8 / 8
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
<unnamed>86.8 %224 / 258100.0 %7 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-detail.html b/mrs_uav_managers/src/index-detail.html new file mode 100644 index 0000000000..4948a31dab --- /dev/null +++ b/mrs_uav_managers/src/index-detail.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1265171873.6 %
Date:2024-04-25 05:42:57Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
<unnamed>84.1 %195 / 232100.0 %8 / 8
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
<unnamed>86.8 %224 / 258100.0 %7 / 7
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
uav_manager.cpp +
68.9%68.9%
+ 		68.9 %805 / 1169100.0 %38 / 38
<unnamed>68.9 %805 / 1169100.0 %38 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-sort-f.html b/mrs_uav_managers/src/index-sort-f.html new file mode 100644 index 0000000000..6fba0601f3 --- /dev/null +++ b/mrs_uav_managers/src/index-sort-f.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1265171873.6 %
Date:2024-04-25 05:42:57Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
uav_manager.cpp +
68.9%68.9%
+ 		68.9 %805 / 1169100.0 %38 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-sort-l.html b/mrs_uav_managers/src/index-sort-l.html new file mode 100644 index 0000000000..af8ae15bf0 --- /dev/null +++ b/mrs_uav_managers/src/index-sort-l.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1265171873.6 %
Date:2024-04-25 05:42:57Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
uav_manager.cpp +
68.9%68.9%
+ 		68.9 %805 / 1169100.0 %38 / 38
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index.html b/mrs_uav_managers/src/index.html new file mode 100644 index 0000000000..e93ad49e9e --- /dev/null +++ b/mrs_uav_managers/src/index.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1265171873.6 %
Date:2024-04-25 05:42:57Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
uav_manager.cpp +
68.9%68.9%
+ 		68.9 %805 / 1169100.0 %38 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html b/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..ca4388e9bc --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-04-25 05:42:57Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::NullTracker::resetStatic()0
mrs_uav_managers::NullTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::NullTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::NullTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_managers::NullTracker::~NullTracker()94
mrs_uav_managers::NullTracker::~NullTracker().294
mrs_uav_managers::NullTracker::deactivate()211
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)215
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_managers::NullTracker::getStatus()6611
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.func.html b/mrs_uav_managers/src/null_tracker.cpp.func.html new file mode 100644 index 0000000000..caed033bef --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.func.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-04-25 05:42:57Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::NullTracker::deactivate()211
mrs_uav_managers::NullTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_managers::NullTracker::resetStatic()0
mrs_uav_managers::NullTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_managers::NullTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::NullTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_managers::NullTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)215
mrs_uav_managers::NullTracker::getStatus()6611
mrs_uav_managers::NullTracker::~NullTracker()94
mrs_uav_managers::NullTracker::~NullTracker().294
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.gcov.frameset.html b/mrs_uav_managers/src/null_tracker.cpp.gcov.frameset.html new file mode 100644 index 0000000000..a33b57e8c3 --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.gcov.html b/mrs_uav_managers/src/null_tracker.cpp.gcov.html new file mode 100644 index 0000000000..dcb2aaa546 --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.gcov.html @@ -0,0 +1,332 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-04-25 05:42:57Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <ros/ros.h>
+       2             : 
+       3             : #include <mrs_uav_managers/tracker.h>
+       4             : 
+       5             : namespace mrs_uav_managers
+       6             : {
+       7             : 
+       8             : /* //{ class NullTracker */
+       9             : 
+      10             : class NullTracker : public mrs_uav_managers::Tracker {
+      11             : 
+      12             : public:
+      13         188 :   ~NullTracker(){};
+      14             : 
+      15             :   bool initialize(const ros::NodeHandle &parent_nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      16             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      17             : 
+      18             :   std::tuple<bool, std::string> activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd);
+      19             :   void                          deactivate(void);
+      20             :   bool                          resetStatic(void);
+      21             : 
+      22             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState &uav_state, const Controller::ControlOutput &last_control_output);
+      23             :   const mrs_msgs::TrackerStatus             getStatus();
+      24             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd);
+      25             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      26             : 
+      27             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd);
+      28             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd);
+      29             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd);
+      30             : 
+      31             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      32             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      33             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      34             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      35             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      36             : 
+      37             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      38             : 
+      39             : private:
+      40             :   ros::NodeHandle nh_;
+      41             :   bool            is_active         = false;
+      42             :   bool            is_initialized    = false;
+      43             :   bool            callbacks_enabled = false;
+      44             : 
+      45             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers;
+      46             : };
+      47             : 
+      48             : //}
+      49             : 
+      50             : // | ------------------- trackers interface ------------------- |
+      51             : 
+      52             : /* //{ initialize() */
+      53             : 
+      54          94 : bool NullTracker::initialize(const ros::NodeHandle &                                                                parent_nh,
+      55             :                              [[maybe_unused]] std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers,
+      56             :                              [[maybe_unused]] std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+      57             : 
+      58         188 :   ros::NodeHandle nh_(parent_nh, "null_tracker");
+      59             : 
+      60          94 :   ros::Time::waitForValid();
+      61             : 
+      62          94 :   is_initialized = true;
+      63             : 
+      64          94 :   this->common_handlers = common_handlers;
+      65             : 
+      66          94 :   ROS_INFO("[NullTracker]: initialized");
+      67             : 
+      68         188 :   return true;
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* //{ activate() */
+      74             : 
+      75         215 : std::tuple<bool, std::string> NullTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+      76             : 
+      77         215 :   std::stringstream ss;
+      78         215 :   ss << "activated";
+      79             : 
+      80         215 :   ROS_INFO_STREAM("[NullTracker]: " << ss.str());
+      81         215 :   is_active = true;
+      82             : 
+      83         430 :   return std::tuple(true, ss.str());
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* //{ deactivate() */
+      89             : 
+      90         211 : void NullTracker::deactivate(void) {
+      91             : 
+      92         211 :   ROS_INFO("[NullTracker]: deactivated");
+      93         211 :   is_active = false;
+      94         211 : }
+      95             : 
+      96             : //}
+      97             : 
+      98             : /* //{ resetStatic() */
+      99             : 
+     100           0 : bool NullTracker::resetStatic(void) {
+     101           0 :   return false;
+     102             : }
+     103             : 
+     104             : //}
+     105             : 
+     106             : /* switchOdometrySource() //{ */
+     107             : 
+     108           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     109           0 :   return std_srvs::TriggerResponse::Ptr();
+     110             : }
+     111             : 
+     112             : //}
+     113             : 
+     114             : /* //{ update() */
+     115             : 
+     116      127134 : std::optional<mrs_msgs::TrackerCommand> NullTracker::update([[maybe_unused]] const mrs_msgs::UavState &       uav_state,
+     117             :                                                             [[maybe_unused]] const Controller::ControlOutput &last_control_output) {
+     118             : 
+     119      127134 :   return {};
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* //{ getStatus() */
+     125             : 
+     126        6611 : const mrs_msgs::TrackerStatus NullTracker::getStatus() {
+     127             : 
+     128        6611 :   mrs_msgs::TrackerStatus tracker_status;
+     129             : 
+     130        6611 :   tracker_status.active            = is_active;
+     131        6611 :   tracker_status.callbacks_enabled = callbacks_enabled;
+     132             : 
+     133        6611 :   return tracker_status;
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* //{ enableCallbacks() */
+     139             : 
+     140         297 : const std_srvs::SetBoolResponse::ConstPtr NullTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     141             : 
+     142         594 :   std_srvs::SetBoolResponse res;
+     143             : 
+     144         297 :   std::stringstream ss;
+     145             : 
+     146         297 :   if (cmd->data != callbacks_enabled) {
+     147             : 
+     148          13 :     callbacks_enabled = cmd->data;
+     149             : 
+     150          13 :     ss << "callbacks " << (callbacks_enabled ? "enabled" : "disabled");
+     151             : 
+     152          13 :     ROS_DEBUG_STREAM("[NullTracker]: " << ss.str());
+     153             : 
+     154             :   } else {
+     155             : 
+     156         284 :     ss << "callbacks were already " << (callbacks_enabled ? "enabled" : "disabled");
+     157             :   }
+     158             : 
+     159         297 :   res.message = ss.str();
+     160         297 :   res.success = true;
+     161             : 
+     162         891 :   return std_srvs::SetBoolResponse::ConstPtr(std::make_unique<std_srvs::SetBoolResponse>(res));
+     163             : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* //{ setReference() */
+     168             : 
+     169           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr NullTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     170           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     171             : }
+     172             : 
+     173             : //}
+     174             : 
+     175             : /* //{ setVelocityReference() */
+     176             : 
+     177           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr NullTracker::setVelocityReference([
+     178             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     179           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     180             : }
+     181             : 
+     182             : //}
+     183             : 
+     184             : /* //{ setTrajectoryReference() */
+     185             : 
+     186           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr NullTracker::setTrajectoryReference([
+     187             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     188           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     189             : }
+     190             : 
+     191             : //}
+     192             : 
+     193             : // | --------------------- other services --------------------- |
+     194             : 
+     195             : /* //{ hover() */
+     196             : 
+     197           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     198           0 :   return std_srvs::TriggerResponse::Ptr();
+     199             : }
+     200             : 
+     201             : //}
+     202             : 
+     203             : /* //{ startTrajectoryTracking() */
+     204             : 
+     205           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     206           0 :   return std_srvs::TriggerResponse::Ptr();
+     207             : }
+     208             : 
+     209             : //}
+     210             : 
+     211             : /* //{ stopTrajectoryTracking() */
+     212             : 
+     213           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     214           0 :   return std_srvs::TriggerResponse::Ptr();
+     215             : }
+     216             : 
+     217             : //}
+     218             : 
+     219             : /* //{ resumeTrajectoryTracking() */
+     220             : 
+     221           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     222           0 :   return std_srvs::TriggerResponse::Ptr();
+     223             : }
+     224             : 
+     225             : //}
+     226             : 
+     227             : /* //{ gotoTrajectoryStart() */
+     228             : 
+     229           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     230           0 :   return std_srvs::TriggerResponse::Ptr();
+     231             : }
+     232             : 
+     233             : //}
+     234             : 
+     235             : /* //{ setConstraints() */
+     236             : 
+     237         325 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr NullTracker::setConstraints([
+     238             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     239             : 
+     240         325 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : }  // namespace mrs_uav_managers
+     246             : 
+     247             : #include <pluginlib/class_list_macros.h>
+     248          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::NullTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html b/mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..2b6e65529b --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html @@ -0,0 +1,82 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/null_tracker.cpp.gcov.png b/mrs_uav_managers/src/null_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..06fdac01e3b61803906bfe6b75c9e4a898089e16 GIT binary patch literal 809 zcmV+^1J?YBP)f0Qq=z*!GdwScxVb8-emMdv2*IGgWYH=J4_C%DkOB)b=Rk+6R}LK)7M|#1w4Az#YN_`iaW( zy69s|e9wsOm0($X>zcGiUK?Cyrr)673n$;>BC;%YK@vXN>V_0hZb>E4pYBZ{d>`k; z*xUH*YrtNO*4Fe5`siH6)xoH3nTPagYy|9q4Y~tBwp`J!zPZGDOW|DSy|Q0< zy#|)h)fwaqXkDwiJ+Rv}6s4uRFqEtq$DQM*uLO0gGP`nyOC+2(Dc<9$p^?2ZY#%RN z8KLJ)rPlRtMe=+;C{d4fhtgMLU=tC5{HU+UBaj@CGJj=eO(p+kT|*wR+&GU~bU#u{ zF2t^6TN_dYWQ(8SjK03r43`=d3NHj&6g17pW_Zwdp6ww;mLvcJcj-s*Uq+nth!lJXTc(c57@>SH4DHe?Wg6skuWVI9cPFeHsrUZY`{c?}wu++^CPB zjdHf*2-%FEeA5y|z1N{bYXt9VX7+1`>DWj$k8fbzhp{15lba1GDgHrIww$ifB#t1* zcOr3=VZtRcP9NPIvp4SRIXbB<-r_c3Wm3e$;U<$J z%D0Y=y3ZbSi;-*Y1LX5FA0T0IPFwiu2XVoX^2?)4B7b?58Ixb1M + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
<unnamed>81.4 %350 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-detail-sort-l.html b/mrs_uav_managers/src/transform_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..26c93ad212 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
<unnamed>81.4 %350 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-detail.html b/mrs_uav_managers/src/transform_manager/index-detail.html new file mode 100644 index 0000000000..13aab0b7b5 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
<unnamed>81.4 %350 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-sort-f.html b/mrs_uav_managers/src/transform_manager/index-sort-f.html new file mode 100644 index 0000000000..2d5a8689c1 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-sort-l.html b/mrs_uav_managers/src/transform_manager/index-sort-l.html new file mode 100644 index 0000000000..ef8180a4f3 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index.html b/mrs_uav_managers/src/transform_manager/index.html new file mode 100644 index 0000000000..9488ac6f5a --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
81.4%81.4%
+ 		81.4 %350 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..0329655d4a --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::transform_manager::TransformManager::getName[abi:cxx11]() const0
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()94
mrs_uav_managers::transform_manager::TransformManager::onInit()94
mrs_uav_managers::transform_manager::TransformManager::TransformManager()94
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1521
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2673
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)72519
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)92705
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)141511
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)158584
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)207659
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)207659
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html new file mode 100644 index 0000000000..cb6a37e21e --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)72519
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2673
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()94
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)158584
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)141511
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)92705
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)207659
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)207659
mrs_uav_managers::transform_manager::TransformManager::onInit()94
mrs_uav_managers::transform_manager::TransformManager::TransformManager()94
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1521
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5
mrs_uav_managers::transform_manager::TransformManager::getName[abi:cxx11]() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..e54ea334d2 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html new file mode 100644 index 0000000000..be942feb86 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html @@ -0,0 +1,1038 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35043081.4 %
Date:2024-04-25 05:42:57Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* //{ includes */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <nodelet/nodelet.h>
+       5             : 
+       6             : #include <mrs_lib/param_loader.h>
+       7             : #include <mrs_lib/subscribe_handler.h>
+       8             : #include <mrs_lib/publisher_handler.h>
+       9             : #include <mrs_lib/attitude_converter.h>
+      10             : #include <mrs_lib/transformer.h>
+      11             : #include <mrs_lib/transform_broadcaster.h>
+      12             : #include <mrs_lib/gps_conversions.h>
+      13             : 
+      14             : #include <mrs_msgs/UavState.h>
+      15             : #include <mrs_msgs/Float64Stamped.h>
+      16             : #include <mrs_msgs/HwApiAltitude.h>
+      17             : #include <mrs_msgs/RtkGps.h>
+      18             : 
+      19             : #include <tf2_ros/transform_broadcaster.h>
+      20             : #include <tf2_ros/static_transform_broadcaster.h>
+      21             : #include <geometry_msgs/TransformStamped.h>
+      22             : 
+      23             : #include <sensor_msgs/NavSatFix.h>
+      24             : 
+      25             : #include <nav_msgs/Odometry.h>
+      26             : 
+      27             : #include <memory>
+      28             : #include <string>
+      29             : 
+      30             : #include <mrs_uav_managers/estimation_manager/support.h>
+      31             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      32             : #include <transform_manager/tf_source.h>
+      33             : #include <transform_manager/tf_mapping_origin.h>
+      34             : 
+      35             : /*//}*/
+      36             : 
+      37             : namespace mrs_uav_managers
+      38             : {
+      39             : 
+      40             : namespace transform_manager
+      41             : {
+      42             : 
+      43             : /*//{ class TransformManager */
+      44             : class TransformManager : public nodelet::Nodelet {
+      45             : 
+      46             :   using Support = estimation_manager::Support;
+      47             : 
+      48             : public:
+      49          94 :   TransformManager() {
+      50          94 :     ch_ = std::make_shared<estimation_manager::CommonHandlers_t>();
+      51             : 
+      52          94 :     ch_->nodelet_name = nodelet_name_;
+      53          94 :     ch_->package_name = package_name_;
+      54          94 :   }
+      55             : 
+      56             :   void onInit();
+      57             :   bool is_initialized_ = false;
+      58             : 
+      59             :   std::string getName() const;
+      60             : 
+      61             :   std::string getPrintName() const;
+      62             : 
+      63             : private:
+      64             :   std::string _custom_config_;
+      65             :   std::string _platform_config_;
+      66             :   std::string _world_config_;
+      67             : 
+      68             :   const std::string package_name_ = "mrs_uav_managers";
+      69             :   const std::string nodelet_name_ = "TransformManager";
+      70             :   const std::string name_         = "transform_manager";
+      71             : 
+      72             :   bool publish_fcu_untilted_tf_;
+      73             : 
+      74             :   std::string ns_local_origin_parent_frame_id_;
+      75             :   std::string ns_local_origin_child_frame_id_;
+      76             :   bool        publish_local_origin_tf_;
+      77             : 
+      78             :   std::string ns_stable_origin_parent_frame_id_;
+      79             :   std::string ns_stable_origin_child_frame_id_;
+      80             :   bool        publish_stable_origin_tf_;
+      81             : 
+      82             :   std::string         ns_fixed_origin_parent_frame_id_;
+      83             :   std::string         ns_fixed_origin_child_frame_id_;
+      84             :   bool                publish_fixed_origin_tf_;
+      85             :   geometry_msgs::PoseStamped pose_first_;
+      86             :   geometry_msgs::Pose pose_fixed_;
+      87             :   geometry_msgs::Pose pose_fixed_diff_;
+      88             : 
+      89             :   std::string ns_amsl_origin_parent_frame_id_;
+      90             :   std::string ns_amsl_origin_child_frame_id_;
+      91             :   bool        publish_amsl_origin_tf_;
+      92             : 
+      93             :   std::string          world_origin_units_;
+      94             :   geometry_msgs::Point world_origin_;
+      95             : 
+      96             :   std::vector<std::string>               tf_source_names_, estimator_names_;
+      97             :   std::vector<std::unique_ptr<TfSource>> tf_sources_;
+      98             : 
+      99             :   std::vector<std::string> utm_source_priority_list_;
+     100             :   std::string              utm_source_name_;
+     101             : 
+     102             :   std::mutex mtx_broadcast_utm_origin_;
+     103             :   std::mutex mtx_broadcast_world_origin_;
+     104             : 
+     105             :   ros::NodeHandle nh_;
+     106             : 
+     107             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     108             : 
+     109             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     110             :   tf2_ros::StaticTransformBroadcaster            static_broadcaster_;
+     111             : 
+     112             :   std::unique_ptr<TfMappingOrigin> tf_mapping_origin_;
+     113             : 
+     114             :   void timeoutCallback(const std::string& topic, const ros::Time& last_msg);
+     115             : 
+     116             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     117             :   void                                          callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     118             :   std::string                                   first_frame_id_;
+     119             :   std::string                                   last_frame_id_;
+     120             :   bool                                          is_first_frame_id_set_        = false;
+     121             :   bool                                          is_local_static_tf_published_ = false;
+     122             : 
+     123             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_height_agl_;
+     124             :   void                                                callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg);
+     125             : 
+     126             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude> sh_altitude_amsl_;
+     127             :   void                                               callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg);
+     128             : 
+     129             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_hw_api_orientation_;
+     130             :   void                                                        callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg);
+     131             : 
+     132             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     133             :   void                                              callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg);
+     134             :   std::atomic<bool>                                 got_utm_offset_ = false;
+     135             : 
+     136             :   mrs_lib::SubscribeHandler<mrs_msgs::RtkGps> sh_rtk_gps_;
+     137             :   void                                        callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg);
+     138             : 
+     139             :   std::optional<geometry_msgs::Pose> transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const;
+     140             : 
+     141             :   void publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg);
+     142             : 
+     143             :   void publishLocalTf();
+     144             : };
+     145             : /*//}*/
+     146             : 
+     147             : 
+     148             : /*//{ onInit() */
+     149          94 : void TransformManager::onInit() {
+     150             : 
+     151          94 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     152             : 
+     153          94 :   ros::Time::waitForValid();
+     154             : 
+     155          94 :   ROS_INFO("[%s]: initializing", getPrintName().c_str());
+     156             : 
+     157          94 :   broadcaster_ = std::make_shared<mrs_lib::TransformBroadcaster>();
+     158             : 
+     159          94 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getPrintName());
+     160          94 :   ch_->transformer->retryLookupNewest(true);
+     161             : 
+     162         188 :   mrs_lib::ParamLoader param_loader(nh_, getPrintName());
+     163             : 
+     164          94 :   param_loader.loadParam("custom_config", _custom_config_);
+     165          94 :   param_loader.loadParam("platform_config", _platform_config_);
+     166          94 :   param_loader.loadParam("world_config", _world_config_);
+     167             : 
+     168          94 :   if (_custom_config_ != "") {
+     169          94 :     param_loader.addYamlFile(_custom_config_);
+     170             :   }
+     171             : 
+     172          94 :   if (_platform_config_ != "") {
+     173          94 :     param_loader.addYamlFile(_platform_config_);
+     174             :   }
+     175             : 
+     176          94 :   if (_world_config_ != "") {
+     177          94 :     param_loader.addYamlFile(_world_config_);
+     178             :   }
+     179             : 
+     180          94 :   param_loader.addYamlFileFromParam("private_config");
+     181          94 :   param_loader.addYamlFileFromParam("public_config");
+     182          94 :   param_loader.addYamlFileFromParam("estimators_config");
+     183             : 
+     184         188 :   const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+     185             : 
+     186          94 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     187             : 
+     188             :   /*//{ initialize scope timer */
+     189          94 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", ch_->scope_timer.enabled);
+     190         188 :   std::string       filepath;
+     191         188 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/transform_manager_scope_timer.txt";
+     192          94 :   ch_->scope_timer.logger                = std::make_shared<mrs_lib::ScopeTimerLogger>(time_logger_filepath, ch_->scope_timer.enabled);
+     193             :   /*//}*/
+     194             : 
+     195             :   /*//{ load world_origin parameters */
+     196             : 
+     197          94 :   bool   is_origin_param_ok = true;
+     198          94 :   double world_origin_x     = 0;
+     199          94 :   double world_origin_y     = 0;
+     200             : 
+     201          94 :   param_loader.loadParam("world_origin/units", world_origin_units_);
+     202             : 
+     203          94 :   if (Support::toLowercase(world_origin_units_) == "utm") {
+     204             : 
+     205           0 :     ROS_INFO("[%s]: Loading world origin in UTM units.", getPrintName().c_str());
+     206             : 
+     207           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", world_origin_x);
+     208           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", world_origin_y);
+     209             : 
+     210          94 :   } else if (Support::toLowercase(world_origin_units_) == "latlon") {
+     211             : 
+     212          94 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getPrintName().c_str());
+     213             : 
+     214             :     double lat, lon;
+     215          94 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     216          94 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     217             : 
+     218          94 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     219             : 
+     220          94 :     ROS_INFO("[%s]: Converted to UTM x: %f, y: %f.", getPrintName().c_str(), world_origin_x, world_origin_y);
+     221             : 
+     222             :   } else {
+     223           0 :     ROS_ERROR("[%s]: world_origin/units must be (\"UTM\"|\"LATLON\"). Got '%s'", getPrintName().c_str(), world_origin_units_.c_str());
+     224           0 :     ros::shutdown();
+     225             :   }
+     226             : 
+     227          94 :   world_origin_.x = world_origin_x;
+     228          94 :   world_origin_.y = world_origin_y;
+     229          94 :   world_origin_.z = 0;
+     230             : 
+     231          94 :   if (!is_origin_param_ok) {
+     232           0 :     ROS_ERROR("[%s]: Could not load all mandatory parameters from world file. Please check your world file.", getPrintName().c_str());
+     233           0 :     ros::shutdown();
+     234             :   }
+     235             : 
+     236             :   /*//}*/
+     237             : 
+     238             :   /*//{ load local_origin parameters */
+     239         188 :   std::string local_origin_parent_frame_id;
+     240          94 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/parent", local_origin_parent_frame_id);
+     241          94 :   ns_local_origin_parent_frame_id_ = ch_->uav_name + "/" + local_origin_parent_frame_id;
+     242             : 
+     243         188 :   std::string local_origin_child_frame_id;
+     244          94 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/child", local_origin_child_frame_id);
+     245          94 :   ns_local_origin_child_frame_id_ = ch_->uav_name + "/" + local_origin_child_frame_id;
+     246             : 
+     247          94 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/enabled", publish_local_origin_tf_);
+     248             :   /*//}*/
+     249             : 
+     250             :   /*//{ load stable_origin parameters */
+     251         188 :   std::string stable_origin_parent_frame_id;
+     252          94 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/parent", stable_origin_parent_frame_id);
+     253          94 :   ns_stable_origin_parent_frame_id_ = ch_->uav_name + "/" + stable_origin_parent_frame_id;
+     254             : 
+     255         188 :   std::string stable_origin_child_frame_id;
+     256          94 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/child", stable_origin_child_frame_id);
+     257          94 :   ns_stable_origin_child_frame_id_ = ch_->uav_name + "/" + stable_origin_child_frame_id;
+     258             : 
+     259          94 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/enabled", publish_stable_origin_tf_);
+     260             :   /*//}*/
+     261             : 
+     262             :   /*//{ load fixed_origin parameters */
+     263         188 :   std::string fixed_origin_parent_frame_id;
+     264          94 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/parent", fixed_origin_parent_frame_id);
+     265          94 :   ns_fixed_origin_parent_frame_id_ = ch_->uav_name + "/" + fixed_origin_parent_frame_id;
+     266             : 
+     267         188 :   std::string fixed_origin_child_frame_id;
+     268          94 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/child", fixed_origin_child_frame_id);
+     269          94 :   ns_fixed_origin_child_frame_id_ = ch_->uav_name + "/" + fixed_origin_child_frame_id;
+     270             : 
+     271          94 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/enabled", publish_fixed_origin_tf_);
+     272             :   /*//}*/
+     273             : 
+     274             :   /*//{ load fcu_untilted parameters */
+     275         188 :   std::string fcu_frame_id;
+     276          94 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/parent", fcu_frame_id);
+     277          94 :   ch_->frames.fcu    = fcu_frame_id;
+     278          94 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + fcu_frame_id;
+     279             : 
+     280         188 :   std::string fcu_untilted_frame_id;
+     281          94 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/child", fcu_untilted_frame_id);
+     282          94 :   ch_->frames.fcu_untilted    = fcu_untilted_frame_id;
+     283          94 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + fcu_untilted_frame_id;
+     284             : 
+     285          94 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/enabled", publish_fcu_untilted_tf_);
+     286             :   /*//}*/
+     287             : 
+     288             :   /*//{ load amsl_origin parameters*/
+     289         188 :   std::string amsl_parent_frame_id, amsl_child_frame_id;
+     290          94 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/enabled", publish_amsl_origin_tf_);
+     291          94 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/parent", amsl_parent_frame_id);
+     292          94 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/child", amsl_child_frame_id);
+     293          94 :   ch_->frames.amsl                = amsl_child_frame_id;
+     294          94 :   ch_->frames.ns_amsl             = ch_->uav_name + "/" + amsl_child_frame_id;
+     295          94 :   ns_amsl_origin_parent_frame_id_ = ch_->uav_name + "/" + amsl_parent_frame_id;
+     296          94 :   ns_amsl_origin_child_frame_id_  = ch_->uav_name + "/" + amsl_child_frame_id;
+     297             : 
+     298             :   /*//}*/
+     299             : 
+     300          94 :   param_loader.loadParam(yaml_prefix + "rtk_antenna/frame_id", ch_->frames.rtk_antenna);
+     301          94 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     302             : 
+     303          94 :   param_loader.loadParam("mrs_uav_managers/estimation_manager/state_estimators", estimator_names_);
+     304          94 :   param_loader.loadParam(yaml_prefix + "tf_sources", tf_source_names_);
+     305             : 
+     306          94 :   param_loader.loadParam(yaml_prefix + "utm_source_priority", utm_source_priority_list_);
+     307         295 :   for (auto utm_source : utm_source_priority_list_) {
+     308         294 :     if (Support::isStringInVector(utm_source, estimator_names_)) {
+     309          93 :       ROS_INFO("[%s]: the source for utm_origin and world origin is: %s", getPrintName().c_str(), utm_source.c_str());
+     310          93 :       utm_source_name_ = utm_source;
+     311          93 :       break;
+     312             :     }
+     313             :   }
+     314             : 
+     315             :   /*//{ initialize tf sources */
+     316          94 :   for (size_t i = 0; i < tf_source_names_.size(); i++) {
+     317             : 
+     318           0 :     const std::string tf_source_name = tf_source_names_[i];
+     319           0 :     const bool        is_utm_source  = tf_source_name == utm_source_name_;
+     320             : 
+     321           0 :     ROS_INFO("[%s]: loading tf source: %s", getPrintName().c_str(), tf_source_name.c_str());
+     322             : 
+     323           0 :     auto source_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/" + tf_source_name);
+     324             : 
+     325           0 :     if (_custom_config_ != "") {
+     326           0 :       source_param_loader->addYamlFile(_custom_config_);
+     327             :     }
+     328             : 
+     329           0 :     if (_platform_config_ != "") {
+     330           0 :       source_param_loader->addYamlFile(_platform_config_);
+     331             :     }
+     332             : 
+     333           0 :     if (_world_config_ != "") {
+     334           0 :       source_param_loader->addYamlFile(_world_config_);
+     335             :     }
+     336             : 
+     337           0 :     source_param_loader->addYamlFileFromParam("private_config");
+     338           0 :     source_param_loader->addYamlFileFromParam("public_config");
+     339           0 :     source_param_loader->addYamlFileFromParam("estimators_config");
+     340             : 
+     341           0 :     tf_sources_.push_back(std::make_unique<TfSource>(tf_source_name, nh_, source_param_loader, broadcaster_, ch_, is_utm_source));
+     342             :   }
+     343             : 
+     344             :   // additionally publish tf of all available estimators
+     345         194 :   for (int i = 0; i < int(estimator_names_.size()); i++) {
+     346             : 
+     347         200 :     const std::string estimator_name = estimator_names_[i];
+     348         100 :     const bool        is_utm_source  = estimator_name == utm_source_name_;
+     349         100 :     ROS_INFO("[%s]: loading tf source of estimator: %s", getPrintName().c_str(), estimator_name.c_str());
+     350             : 
+     351         100 :     auto estimator_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/" + estimator_name);
+     352             : 
+     353         100 :     if (_custom_config_ != "") {
+     354         100 :       estimator_param_loader->addYamlFile(_custom_config_);
+     355             :     }
+     356             : 
+     357         100 :     if (_platform_config_ != "") {
+     358         100 :       estimator_param_loader->addYamlFile(_platform_config_);
+     359             :     }
+     360             : 
+     361         100 :     if (_world_config_ != "") {
+     362         100 :       estimator_param_loader->addYamlFile(_world_config_);
+     363             :     }
+     364             : 
+     365         100 :     estimator_param_loader->addYamlFileFromParam("private_config");
+     366         100 :     estimator_param_loader->addYamlFileFromParam("public_config");
+     367         100 :     estimator_param_loader->addYamlFileFromParam("estimators_config");
+     368             : 
+     369         100 :     tf_sources_.push_back(std::make_unique<TfSource>(estimator_name, nh_, estimator_param_loader, broadcaster_, ch_, is_utm_source));
+     370             :   }
+     371             : 
+     372             :   // initialize mapping_origin tf
+     373             :   bool mapping_origin_tf_enabled;
+     374          94 :   param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled, false);
+     375             : 
+     376          94 :   if (mapping_origin_tf_enabled) {
+     377             : 
+     378           0 :     auto mapping_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/mapping_origin_tf");
+     379             : 
+     380           0 :     if (_custom_config_ != "") {
+     381           0 :       mapping_param_loader->addYamlFile(_custom_config_);
+     382             :     }
+     383             : 
+     384           0 :     if (_platform_config_ != "") {
+     385           0 :       mapping_param_loader->addYamlFile(_platform_config_);
+     386             :     }
+     387             : 
+     388           0 :     if (_world_config_ != "") {
+     389           0 :       mapping_param_loader->addYamlFile(_world_config_);
+     390             :     }
+     391             : 
+     392           0 :     mapping_param_loader->addYamlFileFromParam("private_config");
+     393           0 :     mapping_param_loader->addYamlFileFromParam("public_config");
+     394           0 :     mapping_param_loader->addYamlFileFromParam("estimators_config");
+     395             : 
+     396           0 :     tf_mapping_origin_ = std::make_unique<TfMappingOrigin>(nh_, mapping_param_loader, broadcaster_, ch_);
+     397             :   }
+     398             : 
+     399             :   //}
+     400             : 
+     401             :   /*//{ initialize subscribers */
+     402         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+     403          94 :   shopts.nh                 = nh_;
+     404          94 :   shopts.node_name          = getPrintName();
+     405          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     406          94 :   shopts.threadsafe         = true;
+     407          94 :   shopts.autostart          = true;
+     408          94 :   shopts.queue_size         = 10;
+     409          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     410             : 
+     411          94 :   sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &TransformManager::callbackUavState, this);
+     412             : 
+     413          94 :   sh_height_agl_ = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_agl_in", &TransformManager::callbackHeightAgl, this);
+     414             : 
+     415          94 :   sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in", &TransformManager::callbackAltitudeAmsl, this);
+     416             : 
+     417             :   sh_hw_api_orientation_ =
+     418          94 :       mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "orientation_in", &TransformManager::callbackHwApiOrientation, this);
+     419             : 
+     420          94 :   if (utm_source_name_ == "rtk" || utm_source_name_ == "rtk_garmin") {
+     421           3 :     sh_rtk_gps_ = mrs_lib::SubscribeHandler<mrs_msgs::RtkGps>(shopts, "rtk_gps_in", &TransformManager::callbackRtkGps, this);
+     422             :   } else {
+     423          91 :     sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &TransformManager::callbackGnss, this);
+     424             :   }
+     425             :   /*//}*/
+     426             : 
+     427          94 :   if (!param_loader.loadedSuccessfully()) {
+     428           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     429           0 :     ros::shutdown();
+     430             :   }
+     431             : 
+     432          94 :   is_initialized_ = true;
+     433          94 :   ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     434          94 : }
+     435             : /*//}*/
+     436             : 
+     437             : /*//{ callbackUavState() */
+     438             : 
+     439      158584 : void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+     440             : 
+     441      158584 :   if (!is_initialized_) {
+     442           0 :     return;
+     443             :   }
+     444             : 
+     445      317168 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     446             : 
+     447             :   // obtain first frame_id
+     448      158584 :   if (!is_first_frame_id_set_) {
+     449          94 :     first_frame_id_                = msg->header.frame_id;
+     450          94 :     last_frame_id_                 = msg->header.frame_id;
+     451          94 :     pose_fixed_                    = msg->pose;
+     452          94 :     pose_first_.pose               = msg->pose;
+     453          94 :     pose_first_.header             = msg->header;
+     454          94 :     pose_fixed_diff_.orientation.w = 1;
+     455             : 
+     456          94 :     is_first_frame_id_set_ = true;
+     457             :   }
+     458             : 
+     459             :   // publish static tf from fixed_origin to local_origin based on the first message
+     460      158584 :   if (publish_local_origin_tf_ && !is_local_static_tf_published_) {
+     461          94 :     publishLocalTf();
+     462             :   }
+     463             : 
+     464      158584 :   if (publish_stable_origin_tf_) {
+     465             :     /*//{ publish stable_origin tf*/
+     466      158584 :     geometry_msgs::TransformStamped tf_msg;
+     467      158584 :     tf_msg.header.stamp    = msg->header.stamp;
+     468      158584 :     tf_msg.header.frame_id = ns_stable_origin_parent_frame_id_;
+     469      158584 :     tf_msg.child_frame_id  = ns_stable_origin_child_frame_id_;
+     470             : 
+     471             :     // transform pose to first frame_id
+     472      158584 :     geometry_msgs::PoseStamped pose;
+     473      158584 :     pose.header = msg->header;
+     474      158584 :     pose.pose   = msg->pose;
+     475      158584 :     if (pose.pose.orientation.w == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.x == 0) {
+     476           0 :       ROS_WARN_ONCE("[%s]: Uninitialized quaternion detected during publishing stable_origin tf of %s. Setting w=1", getPrintName().c_str(),
+     477             :                     pose.header.frame_id.c_str());
+     478           0 :       pose.pose.orientation.w = 1.0;
+     479             :     }
+     480             : 
+     481      158584 :     auto res = ch_->transformer->transformSingle(pose, first_frame_id_);
+     482             : 
+     483      158584 :     if (res) {
+     484      158584 :       const tf2::Transform      tf       = Support::tf2FromPose(res->pose);
+     485      158584 :       const tf2::Transform      tf_inv   = tf.inverse();
+     486      158584 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     487      158584 :       tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     488      158584 :       tf_msg.transform.rotation          = pose_inv.orientation;
+     489             : 
+     490      158584 :       if (Support::noNans(tf_msg)) {
+     491             :         try {
+     492      158584 :           broadcaster_->sendTransform(tf_msg);
+     493             :         }
+     494           0 :         catch (...) {
+     495           0 :           ROS_ERROR("exception caught ");
+     496             :         }
+     497             :       } else {
+     498           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     499             :                           tf_msg.child_frame_id.c_str());
+     500             :       }
+     501      158584 :       ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     502             :                     tf_msg.child_frame_id.c_str());
+     503             :     } else {
+     504           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform pose to %s. Not publishing stable_origin transform.", getPrintName().c_str(), first_frame_id_.c_str());
+     505           0 :       return;
+     506             :     }
+     507             :     /*//}*/
+     508             :   }
+     509             : 
+     510      158584 :   if (publish_fixed_origin_tf_) {
+     511             :     /*//{ publish fixed_origin tf*/
+     512      158584 :     if (msg->header.frame_id != last_frame_id_) {
+     513           5 :       ROS_WARN("[%s]: Detected estimator change from %s to %s. Updating offset for fixed origin.", getPrintName().c_str(), last_frame_id_.c_str(),
+     514             :                msg->header.frame_id.c_str());
+     515             : 
+     516           5 :       pose_fixed_diff_ = Support::getPoseDiff(msg->pose, pose_fixed_);
+     517             :     }
+     518             : 
+     519             : 
+     520      158584 :     pose_fixed_ = Support::applyPoseDiff(msg->pose, pose_fixed_diff_);
+     521             : 
+     522      317168 :     geometry_msgs::TransformStamped tf_msg;
+     523      158584 :     tf_msg.header.stamp    = msg->header.stamp;
+     524      158584 :     tf_msg.header.frame_id = ns_fixed_origin_parent_frame_id_;
+     525      158584 :     tf_msg.child_frame_id  = ns_fixed_origin_child_frame_id_;
+     526             : 
+     527      158584 :     const tf2::Transform      tf       = Support::tf2FromPose(pose_fixed_);
+     528      158584 :     const tf2::Transform      tf_inv   = tf.inverse();
+     529      158584 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     530      158584 :     tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     531      158584 :     tf_msg.transform.rotation          = pose_inv.orientation;
+     532             : 
+     533      158584 :     if (Support::noNans(tf_msg)) {
+     534             :       try {
+     535      158584 :         broadcaster_->sendTransform(tf_msg);
+     536             :       }
+     537           0 :       catch (...) {
+     538           0 :         ROS_ERROR("exception caught ");
+     539             :       }
+     540             :     } else {
+     541           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     542             :                         tf_msg.child_frame_id.c_str());
+     543             :     }
+     544      158584 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     545             :                   tf_msg.child_frame_id.c_str());
+     546             :     /*//}*/
+     547             :   }
+     548             : 
+     549             :   /*//{ choose another source of utm and world tfs after estimator switch */
+     550      158584 :   if (msg->header.frame_id != last_frame_id_) {
+     551             : 
+     552          10 :     const std::string last_estimator_name    = Support::frameIdToEstimatorName(last_frame_id_);
+     553          10 :     const std::string current_estimator_name = Support::frameIdToEstimatorName(msg->header.frame_id);
+     554             : 
+     555           5 :     ROS_INFO("[%s]: Detected estimator switch: %s -> %s", getPrintName().c_str(), last_estimator_name.c_str(), current_estimator_name.c_str());
+     556             : 
+     557           5 :     bool   valid_utm_source_found = false;
+     558             :     size_t potential_utm_source_index;
+     559             : 
+     560          12 :     for (size_t i = 0; i < tf_sources_.size(); i++) {
+     561             : 
+     562             :       // first check if tf source can publish utm origin and is not the switched-from estimator
+     563          12 :       if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() != last_estimator_name) {
+     564             : 
+     565           8 :         valid_utm_source_found     = true;
+     566           8 :         potential_utm_source_index = i;
+     567             : 
+     568             :         // check if switched-to estimator is utm_based, if so, use it
+     569           8 :         if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() == current_estimator_name) {
+     570           5 :           potential_utm_source_index = i;
+     571           5 :           break;
+     572             :         }
+     573             :       }
+     574             :     }
+     575             : 
+     576             : 
+     577             :     // if we found a valid utm source, use it, otherwise stay with the switched-from estimator
+     578           5 :     if (valid_utm_source_found) {
+     579             : 
+     580             :       // stop all estimators from publishing utm source
+     581          23 :       for (size_t i = 0; i < tf_sources_.size(); i++) {
+     582          18 :         if (tf_sources_.at(i)->getIsUtmSource()) {
+     583           5 :           tf_sources_.at(i)->setIsUtmSource(false);
+     584           5 :           ROS_INFO("[%s]: setting is_utm_source of estimator %s to false", getPrintName().c_str(), last_estimator_name.c_str());
+     585             :         }
+     586             :       }
+     587             : 
+     588           5 :       tf_sources_.at(potential_utm_source_index)->setIsUtmSource(true);
+     589           5 :       ROS_INFO("[%s]: setting is_utm_source of estimator %s to true", getPrintName().c_str(), current_estimator_name.c_str());
+     590             :     }
+     591             :   }
+     592             :   /*//}*/
+     593             : 
+     594      158584 :   last_frame_id_ = msg->header.frame_id;
+     595             : }
+     596             : /*//}*/
+     597             : 
+     598             : /*//{ callbackHeightAgl() */
+     599             : 
+     600      141511 : void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg) {
+     601             : 
+     602      141511 :   if (!is_initialized_) {
+     603           0 :     return;
+     604             :   }
+     605             : 
+     606      424533 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackHeightAgl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     607             : 
+     608      283022 :   geometry_msgs::TransformStamped tf_msg;
+     609      141511 :   tf_msg.header.stamp    = msg->header.stamp;
+     610      141511 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     611      141511 :   tf_msg.child_frame_id  = msg->header.frame_id;
+     612             : 
+     613      141511 :   tf_msg.transform.translation.x = 0;
+     614      141511 :   tf_msg.transform.translation.y = 0;
+     615      141511 :   tf_msg.transform.translation.z = -msg->value;
+     616      141511 :   tf_msg.transform.rotation.x    = 0;
+     617      141511 :   tf_msg.transform.rotation.y    = 0;
+     618      141511 :   tf_msg.transform.rotation.z    = 0;
+     619      141511 :   tf_msg.transform.rotation.w    = 1;
+     620             : 
+     621      141511 :   if (Support::noNans(tf_msg)) {
+     622             :     try {
+     623      141511 :       broadcaster_->sendTransform(tf_msg);
+     624             :     }
+     625           0 :     catch (...) {
+     626           0 :       ROS_ERROR("exception caught ");
+     627             :     }
+     628             :   } else {
+     629           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     630             :                       tf_msg.child_frame_id.c_str());
+     631             :   }
+     632      141511 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     633             :                 tf_msg.child_frame_id.c_str());
+     634             : }
+     635             : /*//}*/
+     636             : 
+     637             : /*//{ callbackAltitudeAmsl() */
+     638             : 
+     639       92705 : void TransformManager::callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg) {
+     640             : 
+     641       92705 :   if (!is_initialized_) {
+     642           0 :     return;
+     643             :   }
+     644             : 
+     645      278115 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackAltitudeAmsl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     646             : 
+     647      185410 :   geometry_msgs::TransformStamped tf_msg;
+     648       92705 :   tf_msg.header.stamp    = msg->stamp;
+     649       92705 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     650       92705 :   tf_msg.child_frame_id  = ch_->frames.ns_amsl;
+     651             : 
+     652       92705 :   tf_msg.transform.translation.x = 0;
+     653       92705 :   tf_msg.transform.translation.y = 0;
+     654       92705 :   tf_msg.transform.translation.z = -msg->amsl;
+     655       92705 :   tf_msg.transform.rotation.x    = 0;
+     656       92705 :   tf_msg.transform.rotation.y    = 0;
+     657       92705 :   tf_msg.transform.rotation.z    = 0;
+     658       92705 :   tf_msg.transform.rotation.w    = 1;
+     659             : 
+     660       92705 :   if (Support::noNans(tf_msg)) {
+     661             :     try {
+     662       92664 :       broadcaster_->sendTransform(tf_msg);
+     663             :     }
+     664           0 :     catch (...) {
+     665           0 :       ROS_ERROR("exception caught ");
+     666             :     }
+     667             :   } else {
+     668          41 :     ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     669             :                       tf_msg.child_frame_id.c_str());
+     670             :   }
+     671       92705 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     672             :                 tf_msg.child_frame_id.c_str());
+     673             : }
+     674             : /*//}*/
+     675             : 
+     676             : /*//{ callbackHwApiOrientation() */
+     677      207659 : void TransformManager::callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     678             : 
+     679      207659 :   if (!is_initialized_) {
+     680           0 :     return;
+     681             :   }
+     682             : 
+     683      622977 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     684             : 
+     685      207659 :   if (publish_fcu_untilted_tf_) {
+     686      207659 :     publishFcuUntiltedTf(msg);
+     687             :   }
+     688             : }
+     689             : /*//}*/
+     690             : 
+     691             : /*//{ callbackGnss() */
+     692       72519 : void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+     693             : 
+     694       72519 :   if (!is_initialized_) {
+     695       72428 :     return;
+     696             :   }
+     697             : 
+     698       72519 :   if (got_utm_offset_) {
+     699       72428 :     return;
+     700             :   }
+     701             : 
+     702         182 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackGnss", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     703             : 
+     704             :   double out_x;
+     705             :   double out_y;
+     706             : 
+     707          91 :   mrs_lib::UTM(msg->latitude, msg->longitude, &out_x, &out_y);
+     708             : 
+     709          91 :   if (!std::isfinite(out_x)) {
+     710           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     711           0 :     return;
+     712             :   }
+     713             : 
+     714          91 :   if (!std::isfinite(out_y)) {
+     715           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     716           0 :     return;
+     717             :   }
+     718             : 
+     719          91 :   geometry_msgs::Point utm_origin;
+     720          91 :   utm_origin.x = out_x;
+     721          91 :   utm_origin.y = out_y;
+     722          91 :   utm_origin.z = msg->altitude;
+     723             : 
+     724          91 :   ROS_INFO("[%s]: utm_origin position calculated as: x: %.2f, y: %.2f, z: %.2f from GNSS", getPrintName().c_str(), utm_origin.x, utm_origin.y, utm_origin.z);
+     725             : 
+     726         185 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     727          94 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     728          94 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     729             :   }
+     730          91 :   got_utm_offset_ = true;
+     731             : }
+     732             : /*//}*/
+     733             : 
+     734             : /*//{ callbackRtkGps() */
+     735        2673 : void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) {
+     736             : 
+     737        2673 :   if (!is_initialized_) {
+     738        2670 :     return;
+     739             :   }
+     740             : 
+     741        2673 :   if (got_utm_offset_) {
+     742        2668 :     return;
+     743             :   }
+     744             : 
+     745          10 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackRtkGps", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     746             : 
+     747             :   double out_x;
+     748             :   double out_y;
+     749             : 
+     750           5 :   geometry_msgs::PoseStamped rtk_pos;
+     751             : 
+     752           5 :   if (!std::isfinite(msg->gps.latitude)) {
+     753           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->latitude\"!!!", getPrintName().c_str());
+     754           0 :     return;
+     755             :   }
+     756             : 
+     757           5 :   if (!std::isfinite(msg->gps.longitude)) {
+     758           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->longitude\"!!!", getPrintName().c_str());
+     759           0 :     return;
+     760             :   }
+     761             : 
+     762           5 :   if (!std::isfinite(msg->gps.altitude)) {
+     763           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->altitude\"!!!", getPrintName().c_str());
+     764           0 :     return;
+     765             :   }
+     766             : 
+     767           5 :   if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+     768           0 :     ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     769           0 :     return;
+     770             :   }
+     771             : 
+     772           5 :   rtk_pos.header = msg->header;
+     773           5 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+     774           5 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+     775           5 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     776             : 
+     777           5 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+     778           5 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+     779             : 
+     780             : 
+     781             :   // transform the RTK position from antenna to FCU
+     782           5 :   auto res = transformRtkToFcu(rtk_pos);
+     783           5 :   if (res) {
+     784           3 :     rtk_pos.pose = res.value();
+     785             :   } else {
+     786           2 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+     787           2 :     return;
+     788             :   }
+     789             : 
+     790           3 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &out_x, &out_y);
+     791             : 
+     792           3 :   if (!std::isfinite(out_x)) {
+     793           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     794           0 :     return;
+     795             :   }
+     796             : 
+     797           3 :   if (!std::isfinite(out_y)) {
+     798           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     799           0 :     return;
+     800             :   }
+     801             : 
+     802           3 :   geometry_msgs::Point utm_origin;
+     803           3 :   utm_origin.x = out_x;
+     804           3 :   utm_origin.y = out_y;
+     805           3 :   utm_origin.z = msg->gps.altitude;
+     806             : 
+     807           3 :   ROS_INFO("[%s]: utm_origin position calculated as: x: %.2f, y: %.2f, z: %.2f from RTK msg", getPrintName().c_str(), utm_origin.x, utm_origin.y, utm_origin.z);
+     808             : 
+     809           9 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     810           6 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     811           6 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     812             :   }
+     813           3 :   got_utm_offset_ = true;
+     814             : }
+     815             : /*//}*/
+     816             : 
+     817             : /*//{ publishFcuUntiltedTf() */
+     818      207659 : void TransformManager::publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     819             : 
+     820      415318 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     821             : 
+     822             :   double heading;
+     823             : 
+     824             :   try {
+     825      207659 :     heading = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     826             :   }
+     827           0 :   catch (...) {
+     828           0 :     ROS_ERROR("[%s]: Exception caught during getting heading", getPrintName().c_str());
+     829           0 :     return;
+     830             :   }
+     831      207659 :   scope_timer.checkpoint("heading");
+     832             : 
+     833      207659 :   const Eigen::Matrix3d odom_pixhawk_R = mrs_lib::AttitudeConverter(msg->quaternion);
+     834      207659 :   const Eigen::Matrix3d undo_heading_R = mrs_lib::AttitudeConverter(Eigen::AngleAxis(-heading, Eigen::Vector3d(0, 0, 1)));
+     835             : 
+     836      207659 :   const tf2::Quaternion q     = mrs_lib::AttitudeConverter(undo_heading_R * odom_pixhawk_R);
+     837      207659 :   const tf2::Quaternion q_inv = q.inverse();
+     838             : 
+     839      207659 :   scope_timer.checkpoint("q inverse");
+     840             : 
+     841      207659 :   geometry_msgs::TransformStamped tf;
+     842      207659 :   tf.header.stamp            = msg->header.stamp;  // TODO(petrlmat) ros::Time::now()?
+     843      207659 :   tf.header.frame_id         = ch_->frames.ns_fcu;
+     844      207659 :   tf.child_frame_id          = ch_->frames.ns_fcu_untilted;
+     845      207659 :   tf.transform.translation.x = 0.0;
+     846      207659 :   tf.transform.translation.y = 0.0;
+     847      207659 :   tf.transform.translation.z = 0.0;
+     848      207659 :   tf.transform.rotation      = mrs_lib::AttitudeConverter(q_inv);
+     849             : 
+     850      207659 :   scope_timer.checkpoint("tf fill");
+     851      207659 :   if (Support::noNans(tf)) {
+     852      207659 :     broadcaster_->sendTransform(tf);
+     853             :   } else {
+     854           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN encountered in fcu_untilted tf", getPrintName().c_str());
+     855             :   }
+     856      207659 :   scope_timer.checkpoint("tf pub");
+     857             : }
+     858             : /*//}*/
+     859             : 
+     860             : /* publishLocalTf() //{*/
+     861          94 : void TransformManager::publishLocalTf() {
+     862             : 
+     863         282 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     864             : 
+     865          94 :   geometry_msgs::TransformStamped tf_msg;
+     866          94 :   tf_msg.header.stamp = pose_first_.header.stamp;
+     867             : 
+     868          94 :   tf_msg.header.frame_id       = ns_fixed_origin_child_frame_id_;
+     869          94 :   tf_msg.child_frame_id        = ns_local_origin_child_frame_id_;
+     870          94 :   tf_msg.transform.translation = Support::pointToVector3(pose_first_.pose.position);
+     871          94 :   tf_msg.transform.rotation    = pose_first_.pose.orientation;
+     872             : 
+     873          94 :   if (Support::noNans(tf_msg)) {
+     874             : 
+     875             :     try {
+     876          94 :       static_broadcaster_.sendTransform(tf_msg);
+     877             :     }
+     878           0 :     catch (...) {
+     879           0 :       ROS_ERROR("[%s]: exception caught while publishing tf from %s to %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     880             :                 tf_msg.child_frame_id.c_str());
+     881             :     }
+     882             : 
+     883             :   } else {
+     884           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     885             :                       tf_msg.child_frame_id.c_str());
+     886             :   }
+     887          94 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     888             :                 tf_msg.child_frame_id.c_str());
+     889          94 :   is_local_static_tf_published_ = true;
+     890          94 : }
+     891             : /*//}*/
+     892             : 
+     893             : /*//{ transformRtkToFcu() */
+     894           5 : std::optional<geometry_msgs::Pose> TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+     895             : 
+     896          10 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+     897             : 
+     898             :   // inject current orientation into rtk pose
+     899          10 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+     900           5 :   if (res1) {
+     901           3 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+     902             :   } else {
+     903           2 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s.", getPrintName().c_str(), ch_->frames.ns_fcu_untilted.c_str(),
+     904             :                        ch_->frames.ns_fcu.c_str());
+     905           2 :     return {};
+     906             :   }
+     907             : 
+     908             :   // invert tf
+     909           3 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+     910           6 :   geometry_msgs::PoseStamped utm_in_antenna;
+     911           3 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+     912           3 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+     913           3 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+     914             : 
+     915             :   // transform to fcu
+     916           6 :   geometry_msgs::PoseStamped utm_in_fcu;
+     917           3 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+     918           3 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+     919           6 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+     920             : 
+     921           3 :   if (res2) {
+     922           3 :     utm_in_fcu = res2.value();
+     923             :   } else {
+     924           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform RTK pose from %s to %s.", getPrintName().c_str(), utm_in_antenna.header.frame_id.c_str(),
+     925             :                        ch_->frames.ns_fcu.c_str());
+     926           0 :     return {};
+     927             :   }
+     928             : 
+     929             :   // invert tf
+     930           3 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+     931           3 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+     932             : 
+     933           3 :   return fcu_in_utm;
+     934             : }
+     935             : /*//}*/
+     936             : 
+     937             : /*//{ getName() */
+     938           0 : std::string TransformManager::getName() const {
+     939           0 :   return name_;
+     940             : }
+     941             : /*//}*/
+     942             : 
+     943             : /*//{ getPrintName() */
+     944        1521 : std::string TransformManager::getPrintName() const {
+     945        1521 :   return nodelet_name_;
+     946             : }
+     947             : /*//}*/
+     948             : 
+     949             : }  // namespace transform_manager
+     950             : 
+     951             : }  // namespace mrs_uav_managers
+     952             : 
+     953             : #include <pluginlib/class_list_macros.h>
+     954          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::transform_manager::TransformManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..c43d32dcaf --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html @@ -0,0 +1,259 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..71b886c1074540f2c30facd83811f08d6f812436 GIT binary patch literal 3383 zcmV-74ao9|P)^F{Q4v89?d?9i{=47D z`|YgqqUK57TOpueIYo|bSXGC-j6lod@p=bum*a|2O4F(?V2;W9C zABCo}KvC9qwebisj^bf@g<`X#W?*;KTrmR?m;-iIeW)|%aVAlGDR4u>POg_$t&zW? z>JijE$Q3};0AC!e`#xq;M`gk;6w{K*JZa@NO^b7jQd|bw%0@ALH4#1vbmpp!z+0TH z7LE^4C=QHWp_nGacuW`=ia`qqxDLudtZ zwbw&2MBrxNZ!j4n}0*=ewIwlM=cP-$ZTNUD|buz@scrRL-%ifFe`& zRw9raD$dt6`nMZW+*9>&9~zC{kodAa4e7a)xXA39J+1G@MV`Q`y6ZY{gzvPf-d>It z;6}J-9C`Kt4viUTAkh`k6RXZ5FgB_-1$VlthCL5sZ6b3*)g$eL8rCX&DzDj)(LJs_ zJ6b5Tky{POF)h%-W9%8&=Prs^OoYDeqp0KT1kaAnm`h}c63K|=<@+N?Uh<+lTze^2 z;3&{e5f%;DOEEQc7Mj^jaYo!c79vUC6lkV*343HV?PB;$9}ckxbA;hI-X%sX(M*>=Yl5pHQCR|LxEE^nZTV zpHf~xTPYMhA-+5VMKjP?^=s5NUCT1v1wrX$I9wsQ@W+#y7c}8IG{wzpiT08lSOuX^ zp+!Bw-d=CtD~o~j0D*h>7)-VcVhaU&?&0HT+-)pkwgRbw68EI?*pl$4Q677=S>TSV zcue@X1K1iFBg2vIQ8r33!`Ih;QJp;Fd(Wt7-pTMwG6rlWZo;!k9LB~{L34Z(?)4Xb zrPp$iHp)P;jhR9POakm2UeFy{#9sS}(%w?CXNs*BT1ngrt9ISnv5rks%r3BK1q9i*9UHf-PkGdUQ zF&sR0bq1+DbFVdEt$R{q+HG_QNCvU)j4;}+0?HJvLUI0TuffOsOn`y{f3faxRG}(L z8%y9MP~2a&7`$@nH4#J_Eo37ah0h`{XHZRvMJfMwgBh@zkVBm#q+`^6`d9964 z0=%G&Y5`TNxTh7Ep}V54LRn*!>w;KHSuu~BrW(uo8R5K=dg&)ew{VD->I*C4%@3gc zTn*fwB!{R< zst))rN_aLaQzyEyB#a|(8se&*5bQxf7b&)OAam7CV5BIgUM{d%^*YLBps^&1+M!PW zQGbX;PJj#_?+Ci+M^AgU&Ve0Nx6miUTETTy4Q%1;;Nm+!%@H`Y`s`w_X^2Ct+EdiW zWnQ{gDHntlnZzw_4@uf%PwRs9r%JMEY1~Vb8Z?&DEMzp~2xuIb>9*Kn`yC5Sm|1@` zcP*5Ki+s|(xw$KC2e@1+!@W=##`{8{$bP#p!F#mXP_8Y|tHxU$G9SxkTqsa!u)xU% zEQ1n&)gS>^$_!jk)f8DfXYs_q`vrjx2(2n+ob7Zh(A7Xl(aguNULO2lE4SZ}UW)=r*A#Nq zDd*bhxN7@q+kJ7|YwYf=;7>}ljmdGqLlT^=teO$x`IHR&1rB70XtFCQ7)$L_DYIo6 z7gnU;0+j9lsv(X8Zu#O&W(5YuCrYsZ$-8{|JO=tX!Wo7h_SkHfr>NV0SascY9%xFF z2qL>BcIx7GBf{-vpZkgjT10mfNn0{7t~plipHhPA>H|wSPC;W2v})s=0qmZOsD*8X zm{vZcbq^UEdTh{~0gb!xEIz|e*c4jhiwJD9yho*2U8K`vE@Z}N2Re~;o#i+M&VY6j zWt6LsMmr3$ALDd%AjbfR5@Dh<_Fh-n4^--MaR2O`ijFo$zESN{-pG8kwei>2Jv^{p z_v}3?(Xw#S#F;D{1}YY=<|AA6;+0g@OV;(-wWsNex?gCpaM2c4g$LS#1rM8rdk{M# zS~w3hY0}$HWiMJdyfvg)IA3+wybi-xM2%WF5c(uzQz_CnnvYw)nI*ZKL-0b4KODcb#Mhm~ILFwA`l8IqIJ+dF zs0_U1lGct71fsCm9Hq^xtf-{U77Ak#x>9wE_zGNC(mvPuS;&`V#S3BZ(nj+}&PB&G zQxecA_ulavIrW?0jht4m$pl&IB_RQ-P8_C4v$!On(}Z__X9O#J2*`Jv9;%v*6n__J zEM1#+7PnAH=H!7!jcarZAJr~^;1>1-*!pilP~Acl`qbRQhuzK?-NK?N$k@~p{yqzb z%*Q#I0D}3bPK|%fN2}L0|E%VtO>vL;I1&ROow>|>w18jpQCGcGg$Mp2^MT{xOFV>` zZdJ+~8z`-j@Q4ImzUD(UAd*#}cHwIk%)M57PWL_(iq=G+j}WuQ3WXE1is#oPA$o?J zHgc}ayi#*c-rL5rr^4-S{lObEu)i}->dfi zRA+$VP*iCGZVOR$_Ur;K+QjW&hr8UL^puY;Y!6+>IZT^UOj3i;CjnS9zT>CCQe~~o z{m+Qw>NbvDNnvH`x&JkBzu!hY9Dm5f%_38GFI~m4tgJ$b?cr;DKgYz$ljoo9%pVz< z>ZHWXcVRee1_31%`8vFo%*^`f1xv_ zC*p*ee#4a#;a^wE*z148mD0VbE4C^zjniC!GkIkF$-!V1)LR;2(w9|wj5|Z{zLa>Z zC!O=juP<9@)>B3rYwHk#;|l4oa16l=+*7sV?ih(9OsbZTljVr8s z$}ghCt5)|a0C{EcRuh*le;XU58576G_)ax(UiPldbirQj37z3jhdBo5tia3)t`PlL zQQXxTb8?80qIs<_8Gk$R+AI>VR(NI|;4=Z?<$*uAR2AGVI4beKOlu?v(Pj9Qdyv%h z`AW2fB>5xO)#2Ua475^AR|nnm$)+ue#R$L@+)8%J=HXhrlA0TvLb;7}sBtO4Bf+aV zA0Tl{ef*N1JZIhJxqU_cRTuqOj%Q4WUuDmWN*g0Roa|tu8E6U?iOE*1hmTNX8wVL~ zDyC#`~RIaxPJoR531Y`45cgO!hNBJeB|e N002ovPDHLkV1i7?YgGUM literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..1412f21fb9 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:805116968.9 %
Date:2024-04-25 05:42:57Functions:3838100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::elandSrv()1
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::callbackMinHeightCheck(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()2
mrs_uav_managers::uav_manager::UavManager::landSrv()5
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()5
mrs_uav_managers::uav_manager::UavManager::disarmSrv()7
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()9
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)17
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()20
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)22
mrs_uav_managers::uav_manager::UavManager::ungripSrv()23
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)66
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)67
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()68
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)68
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)71
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::uav_manager::UavManager::initialize()94
mrs_uav_managers::uav_manager::UavManager::preinitialize()94
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)94
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)94
mrs_uav_managers::uav_manager::UavManager::onInit()94
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)98
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)113
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)183
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)184
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)283
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)785
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)971
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)2055
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)20591
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)20660
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)52060
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)72444
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)158554
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.func.html b/mrs_uav_managers/src/uav_manager.cpp.func.html new file mode 100644 index 0000000000..cb01ae19de --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.func.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:805116968.9 %
Date:2024-04-25 05:42:57Functions:3838100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_managers::uav_manager::UavManager::initialize()94
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()20
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)67
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)785
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)971
mrs_uav_managers::uav_manager::UavManager::preinitialize()94
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)20660
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)20591
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)22
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)283
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)158554
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)184
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)2055
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)52060
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)72444
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)17
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()9
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)183
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)71
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()68
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)94
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)66
mrs_uav_managers::uav_manager::UavManager::callbackMinHeightCheck(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)94
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)98
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)113
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)68
mrs_uav_managers::uav_manager::UavManager::onInit()94
mrs_uav_managers::uav_manager::UavManager::landSrv()5
mrs_uav_managers::uav_manager::UavManager::elandSrv()1
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()5
mrs_uav_managers::uav_manager::UavManager::disarmSrv()7
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()2
mrs_uav_managers::uav_manager::UavManager::ungripSrv()23
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..f6c3894100 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.html b/mrs_uav_managers/src/uav_manager.cpp.gcov.html new file mode 100644 index 0000000000..297826f94a --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.html @@ -0,0 +1,2765 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:805116968.9 %
Date:2024-04-25 05:42:57Functions:3838100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <nodelet/nodelet.h>
+       5             : 
+       6             : #include <std_srvs/SetBool.h>
+       7             : #include <std_srvs/Trigger.h>
+       8             : #include <std_msgs/String.h>
+       9             : #include <std_msgs/Float64.h>
+      10             : 
+      11             : #include <geometry_msgs/Vector3Stamped.h>
+      12             : 
+      13             : #include <nav_msgs/Odometry.h>
+      14             : #include <mrs_msgs/Vec1.h>
+      15             : #include <mrs_msgs/Vec4.h>
+      16             : #include <mrs_msgs/String.h>
+      17             : #include <mrs_msgs/TrackerCommand.h>
+      18             : #include <mrs_msgs/Float64Stamped.h>
+      19             : #include <mrs_msgs/Float64.h>
+      20             : #include <mrs_msgs/BoolStamped.h>
+      21             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      22             : #include <mrs_msgs/ReferenceStampedSrv.h>
+      23             : #include <mrs_msgs/ConstraintManagerDiagnostics.h>
+      24             : #include <mrs_msgs/GainManagerDiagnostics.h>
+      25             : #include <mrs_msgs/UavManagerDiagnostics.h>
+      26             : #include <mrs_msgs/EstimationDiagnostics.h>
+      27             : #include <mrs_msgs/HwApiStatus.h>
+      28             : #include <mrs_msgs/ControllerDiagnostics.h>
+      29             : #include <mrs_msgs/HwApiCapabilities.h>
+      30             : 
+      31             : #include <sensor_msgs/NavSatFix.h>
+      32             : 
+      33             : #include <mrs_lib/profiler.h>
+      34             : #include <mrs_lib/scope_timer.h>
+      35             : #include <mrs_lib/param_loader.h>
+      36             : #include <mrs_lib/mutex.h>
+      37             : #include <mrs_lib/transformer.h>
+      38             : #include <mrs_lib/attitude_converter.h>
+      39             : #include <mrs_lib/subscribe_handler.h>
+      40             : #include <mrs_lib/publisher_handler.h>
+      41             : #include <mrs_lib/service_client_handler.h>
+      42             : #include <mrs_lib/msg_extractor.h>
+      43             : #include <mrs_lib/geometry/cyclic.h>
+      44             : #include <mrs_lib/geometry/misc.h>
+      45             : #include <mrs_lib/quadratic_throttle_model.h>
+      46             : 
+      47             : //}
+      48             : 
+      49             : /* using //{ */
+      50             : 
+      51             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      52             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      53             : 
+      54             : using radians  = mrs_lib::geometry::radians;
+      55             : using sradians = mrs_lib::geometry::sradians;
+      56             : 
+      57             : //}
+      58             : 
+      59             : namespace mrs_uav_managers
+      60             : {
+      61             : 
+      62             : namespace uav_manager
+      63             : {
+      64             : 
+      65             : /* //{ class UavManager */
+      66             : 
+      67             : // state machine
+      68             : typedef enum
+      69             : {
+      70             : 
+      71             :   IDLE_STATE,
+      72             :   GOTO_STATE,
+      73             :   LANDING_STATE,
+      74             : 
+      75             : } LandingStates_t;
+      76             : 
+      77             : const char* state_names[3] = {
+      78             : 
+      79             :     "IDLING", "GOTO", "LANDING"};
+      80             : 
+      81             : class UavManager : public nodelet::Nodelet {
+      82             : 
+      83             : private:
+      84             :   ros::NodeHandle nh_;
+      85             :   bool            is_initialized_ = false;
+      86             :   std::string     _uav_name_;
+      87             : 
+      88             : public:
+      89             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+      90             : 
+      91             : public:
+      92             :   bool                                       scope_timer_enabled_ = false;
+      93             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+      94             : 
+      95             : public:
+      96             :   virtual void onInit();
+      97             : 
+      98             :   // | ------------------------- HW API ------------------------- |
+      99             : 
+     100             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     101             : 
+     102             :   // this timer will check till we already got the hardware api diagnostics
+     103             :   // then it will trigger the initialization of the controllers and finish
+     104             :   // the initialization of the ControlManager
+     105             :   ros::Timer timer_hw_api_capabilities_;
+     106             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     107             : 
+     108             :   void preinitialize(void);
+     109             :   void initialize(void);
+     110             : 
+     111             :   mrs_msgs::HwApiCapabilities hw_api_capabilities_;
+     112             : 
+     113             :   // | ----------------------- subscribers ---------------------- |
+     114             : 
+     115             :   mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics>        sh_controller_diagnostics_;
+     116             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry>                     sh_odometry_;
+     117             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>        sh_estimation_diagnostics_;
+     118             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>    sh_control_manager_diag_;
+     119             :   mrs_lib::SubscribeHandler<std_msgs::Float64>                      sh_mass_estimate_;
+     120             :   mrs_lib::SubscribeHandler<std_msgs::Float64>                      sh_throttle_;
+     121             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>               sh_height_;
+     122             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>                  sh_hw_api_status_;
+     123             :   mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics>       sh_gains_diag_;
+     124             :   mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics> sh_constraints_diag_;
+     125             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>                 sh_hw_api_gnss_;
+     126             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>               sh_max_height_;
+     127             :   mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>               sh_tracker_cmd_;
+     128             : 
+     129             :   void callbackHwApiGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     130             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     131             : 
+     132             :   // service servers
+     133             :   ros::ServiceServer service_server_takeoff_;
+     134             :   ros::ServiceServer service_server_land_;
+     135             :   ros::ServiceServer service_server_land_home_;
+     136             :   ros::ServiceServer service_server_land_there_;
+     137             :   ros::ServiceServer service_server_midair_activation_;
+     138             :   ros::ServiceServer service_server_min_height_check_;
+     139             : 
+     140             :   // service callbacks
+     141             :   bool callbackTakeoff(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     142             :   bool callbackLand(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     143             :   bool callbackLandHome(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     144             :   bool callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     145             : 
+     146             :   // service clients
+     147             :   mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>                sch_takeoff_;
+     148             :   mrs_lib::ServiceClientHandler<mrs_msgs::String>              sch_switch_tracker_;
+     149             :   mrs_lib::ServiceClientHandler<mrs_msgs::String>              sch_switch_controller_;
+     150             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_land_;
+     151             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_eland_;
+     152             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_ehover_;
+     153             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_control_callbacks_;
+     154             :   mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> sch_emergency_reference_;
+     155             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_arm_;
+     156             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_odometry_callbacks_;
+     157             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_ungrip_;
+     158             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_toggle_control_output_;
+     159             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_offboard_;
+     160             : 
+     161             :   // service client wrappers
+     162             :   bool takeoffSrv(void);
+     163             :   bool switchTrackerSrv(const std::string& tracker);
+     164             :   bool switchControllerSrv(const std::string& controller);
+     165             :   bool landSrv(void);
+     166             :   bool elandSrv(void);
+     167             :   bool ehoverSrv(void);
+     168             :   void disarmSrv(void);
+     169             :   bool emergencyReferenceSrv(const mrs_msgs::ReferenceStamped& goal);
+     170             :   void setOdometryCallbacksSrv(const bool& input);
+     171             :   void setControlCallbacksSrv(const bool& input);
+     172             :   void ungripSrv(void);
+     173             :   bool toggleControlOutput(const bool& input);
+     174             :   bool offboardSrv(const bool in);
+     175             : 
+     176             :   ros::Timer timer_takeoff_;
+     177             :   ros::Timer timer_max_height_;
+     178             :   ros::Timer timer_min_height_;
+     179             :   ros::Timer timer_landing_;
+     180             :   ros::Timer timer_maxthrottle_;
+     181             :   ros::Timer timer_flighttime_;
+     182             :   ros::Timer timer_diagnostics_;
+     183             :   ros::Timer timer_midair_activation_;
+     184             : 
+     185             :   // timer callbacks
+     186             :   void timerLanding(const ros::TimerEvent& event);
+     187             :   void timerTakeoff(const ros::TimerEvent& event);
+     188             :   void timerMaxHeight(const ros::TimerEvent& event);
+     189             :   void timerMinHeight(const ros::TimerEvent& event);
+     190             :   void timerFlightTime(const ros::TimerEvent& event);
+     191             :   void timerMaxthrottle(const ros::TimerEvent& event);
+     192             :   void timerDiagnostics(const ros::TimerEvent& event);
+     193             : 
+     194             :   // publishers
+     195             :   mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics> ph_diag_;
+     196             : 
+     197             :   // max height checking
+     198             :   bool              _max_height_enabled_ = false;
+     199             :   double            _max_height_checking_rate_;
+     200             :   double            _max_height_offset_;
+     201             :   std::atomic<bool> fixing_max_height_ = false;
+     202             : 
+     203             :   // min height checking
+     204             :   std::atomic<bool> min_height_check_ = false;
+     205             :   double            _min_height_checking_rate_;
+     206             :   double            _min_height_offset_;
+     207             :   double            _min_height_;
+     208             :   std::atomic<bool> fixing_min_height_ = false;
+     209             : 
+     210             :   // mass estimation during landing
+     211             :   double    throttle_mass_estimate_;
+     212             :   bool      throttle_under_threshold_ = false;
+     213             :   ros::Time throttle_mass_estimate_first_time_;
+     214             : 
+     215             :   // velocity during landing
+     216             :   bool      velocity_under_threshold_ = false;
+     217             :   ros::Time velocity_under_threshold_first_time_;
+     218             : 
+     219             :   bool _gain_manager_required_       = false;
+     220             :   bool _constraint_manager_required_ = false;
+     221             : 
+     222             :   std::tuple<bool, std::string> landImpl(void);
+     223             :   std::tuple<bool, std::string> landWithDescendImpl(void);
+     224             :   std::tuple<bool, std::string> midairActivationImpl(void);
+     225             : 
+     226             :   // saved takeoff coordinates and allows to land there again
+     227             :   mrs_msgs::ReferenceStamped land_there_reference_;
+     228             :   std::mutex                 mutex_land_there_reference_;
+     229             : 
+     230             :   // to which height to takeoff
+     231             :   double _takeoff_height_;
+     232             : 
+     233             :   std::atomic<bool> takeoff_successful_ = false;
+     234             : 
+     235             :   // names of important trackers
+     236             :   std::string _null_tracker_name_;
+     237             : 
+     238             :   // takeoff timer
+     239             :   double     _takeoff_timer_rate_;
+     240             :   bool       takingoff_            = false;
+     241             :   int        number_of_takeoffs_   = 0;
+     242             :   double     last_mass_difference_ = 0;
+     243             :   std::mutex mutex_last_mass_difference_;
+     244             :   bool       waiting_for_takeoff_ = false;
+     245             : 
+     246             :   // after takeoff
+     247             :   std::string _after_takeoff_tracker_name_;
+     248             :   std::string _after_takeoff_controller_name_;
+     249             :   std::string _takeoff_tracker_name_;
+     250             :   std::string _takeoff_controller_name_;
+     251             : 
+     252             :   // Landing timer
+     253             :   std::string _landing_tracker_name_;
+     254             :   std::string _landing_controller_name_;
+     255             :   double      _landing_cutoff_mass_factor_;
+     256             :   double      _landing_cutoff_mass_timeout_;
+     257             :   double      _landing_timer_rate_;
+     258             :   double      _landing_descend_height_;
+     259             :   bool        landing_ = false;
+     260             :   double      _uav_mass_;
+     261             :   double      _g_;
+     262             :   double      landing_uav_mass_;
+     263             :   bool        _landing_disarm_ = false;
+     264             :   double      _landing_tracking_tolerance_translation_;
+     265             :   double      _landing_tracking_tolerance_heading_;
+     266             : 
+     267             :   // diagnostics timer
+     268             :   double _diagnostics_timer_rate_;
+     269             : 
+     270             :   mrs_lib::quadratic_throttle_model::MotorParams_t _throttle_model_;
+     271             : 
+     272             :   // landing state machine states
+     273             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     274             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     275             : 
+     276             :   // timer for checking max flight time
+     277             :   double     _flighttime_timer_rate_;
+     278             :   double     _flighttime_max_time_;
+     279             :   bool       _flighttime_timer_enabled_ = false;
+     280             :   double     flighttime_                = 0;
+     281             :   std::mutex mutex_flighttime_;
+     282             : 
+     283             :   // timer for checking maximum throttle
+     284             :   bool      _maxthrottle_timer_enabled_ = false;
+     285             :   double    _maxthrottle_timer_rate_;
+     286             :   double    _maxthrottle_max_throttle_;
+     287             :   double    _maxthrottle_eland_timeout_;
+     288             :   double    _maxthrottle_ungrip_timeout_;
+     289             :   bool      maxthrottle_above_threshold_ = false;
+     290             :   ros::Time maxthrottle_first_time_;
+     291             : 
+     292             :   // profiler
+     293             :   mrs_lib::Profiler profiler_;
+     294             :   bool              _profiler_enabled_ = false;
+     295             : 
+     296             :   // midair activation
+     297             :   void      timerMidairActivation(const ros::TimerEvent& event);
+     298             :   bool      callbackMidairActivation(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     299             :   ros::Time midair_activation_started_;
+     300             : 
+     301             :   bool callbackMinHeightCheck(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     302             : 
+     303             :   double      _midair_activation_timer_rate_;
+     304             :   std::string _midair_activation_during_controller_;
+     305             :   std::string _midair_activation_during_tracker_;
+     306             :   std::string _midair_activation_after_controller_;
+     307             :   std::string _midair_activation_after_tracker_;
+     308             : 
+     309             :   void changeLandingState(LandingStates_t new_state);
+     310             : };
+     311             : 
+     312             : //}
+     313             : 
+     314             : /* onInit() //{ */
+     315             : 
+     316          94 : void UavManager::onInit() {
+     317          94 :   preinitialize();
+     318          94 : }
+     319             : 
+     320             : //}
+     321             : 
+     322             : /* preinitialize() //{ */
+     323             : 
+     324          94 : void UavManager::preinitialize(void) {
+     325             : 
+     326          94 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     327             : 
+     328          94 :   ros::Time::waitForValid();
+     329             : 
+     330          94 :   mrs_lib::SubscribeHandlerOptions shopts;
+     331          94 :   shopts.nh                 = nh_;
+     332          94 :   shopts.node_name          = "UavManager";
+     333          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     334          94 :   shopts.threadsafe         = true;
+     335          94 :   shopts.autostart          = true;
+     336          94 :   shopts.queue_size         = 10;
+     337          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     338             : 
+     339          94 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     340             : 
+     341          94 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &UavManager::timerHwApiCapabilities, this);
+     342          94 : }
+     343             : 
+     344             : //}
+     345             : 
+     346             : /* initialize() //{ */
+     347             : 
+     348          94 : void UavManager::initialize() {
+     349             : 
+     350          94 :   ROS_INFO("[UavManager]: initializing");
+     351             : 
+     352         188 :   mrs_lib::ParamLoader param_loader(nh_, "UavManager");
+     353             : 
+     354         188 :   std::string custom_config_path;
+     355         188 :   std::string platform_config_path;
+     356         188 :   std::string world_config_path;
+     357             : 
+     358          94 :   param_loader.loadParam("custom_config", custom_config_path);
+     359          94 :   param_loader.loadParam("platform_config", platform_config_path);
+     360          94 :   param_loader.loadParam("world_config", world_config_path);
+     361             : 
+     362          94 :   if (custom_config_path != "") {
+     363          94 :     param_loader.addYamlFile(custom_config_path);
+     364             :   }
+     365             : 
+     366          94 :   if (platform_config_path != "") {
+     367          94 :     param_loader.addYamlFile(platform_config_path);
+     368             :   }
+     369             : 
+     370          94 :   if (world_config_path != "") {
+     371          94 :     param_loader.addYamlFile(world_config_path);
+     372             :   }
+     373             : 
+     374          94 :   param_loader.addYamlFileFromParam("private_config");
+     375          94 :   param_loader.addYamlFileFromParam("public_config");
+     376             : 
+     377         188 :   const std::string yaml_prefix = "mrs_uav_managers/uav_manager/";
+     378             : 
+     379             :   // params passed from the launch file are not prefixed
+     380          94 :   param_loader.loadParam("uav_name", _uav_name_);
+     381          94 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     382          94 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     383          94 :   param_loader.loadParam("g", _g_);
+     384             : 
+     385             :   // motor params are also not prefixed, since they are common to more nodes
+     386          94 :   param_loader.loadParam("motor_params/n_motors", _throttle_model_.n_motors);
+     387          94 :   param_loader.loadParam("motor_params/a", _throttle_model_.A);
+     388          94 :   param_loader.loadParam("motor_params/b", _throttle_model_.B);
+     389             : 
+     390          94 :   param_loader.loadParam(yaml_prefix + "null_tracker", _null_tracker_name_);
+     391             : 
+     392          94 :   param_loader.loadParam(yaml_prefix + "takeoff/rate", _takeoff_timer_rate_);
+     393          94 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/tracker", _after_takeoff_tracker_name_);
+     394          94 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/controller", _after_takeoff_controller_name_);
+     395          94 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/controller", _takeoff_controller_name_);
+     396          94 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/tracker", _takeoff_tracker_name_);
+     397          94 :   param_loader.loadParam(yaml_prefix + "takeoff/takeoff_height", _takeoff_height_);
+     398             : 
+     399          94 :   if (_takeoff_height_ < 0.5 || _takeoff_height_ > 10.0) {
+     400           0 :     ROS_ERROR("[UavManager]: the takeoff height (%.2f m) has to be between 0.5 and 10 meters", _takeoff_height_);
+     401           0 :     ros::shutdown();
+     402             :   }
+     403             : 
+     404          94 :   param_loader.loadParam(yaml_prefix + "landing/rate", _landing_timer_rate_);
+     405          94 :   param_loader.loadParam(yaml_prefix + "landing/landing_tracker", _landing_tracker_name_);
+     406          94 :   param_loader.loadParam(yaml_prefix + "landing/landing_controller", _landing_controller_name_);
+     407          94 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_mass_factor", _landing_cutoff_mass_factor_);
+     408          94 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_timeout", _landing_cutoff_mass_timeout_);
+     409          94 :   param_loader.loadParam(yaml_prefix + "landing/disarm", _landing_disarm_);
+     410          94 :   param_loader.loadParam(yaml_prefix + "landing/descend_height", _landing_descend_height_);
+     411          94 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/translation", _landing_tracking_tolerance_translation_);
+     412          94 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/heading", _landing_tracking_tolerance_heading_);
+     413             : 
+     414          94 :   param_loader.loadParam(yaml_prefix + "midair_activation/rate", _midair_activation_timer_rate_);
+     415          94 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/controller", _midair_activation_during_controller_);
+     416          94 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/tracker", _midair_activation_during_tracker_);
+     417          94 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/controller", _midair_activation_after_controller_);
+     418          94 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/tracker", _midair_activation_after_tracker_);
+     419             : 
+     420          94 :   param_loader.loadParam(yaml_prefix + "max_height_checking/enabled", _max_height_enabled_);
+     421          94 :   param_loader.loadParam(yaml_prefix + "max_height_checking/rate", _max_height_checking_rate_);
+     422          94 :   param_loader.loadParam(yaml_prefix + "max_height_checking/safety_height_offset", _max_height_offset_);
+     423             : 
+     424             :   {
+     425             :     bool tmp;
+     426             : 
+     427          94 :     param_loader.loadParam(yaml_prefix + "min_height_checking/enabled", tmp);
+     428             : 
+     429          94 :     min_height_check_ = tmp;
+     430             :   }
+     431             : 
+     432          94 :   param_loader.loadParam(yaml_prefix + "min_height_checking/rate", _min_height_checking_rate_);
+     433          94 :   param_loader.loadParam(yaml_prefix + "min_height_checking/safety_height_offset", _min_height_offset_);
+     434          94 :   param_loader.loadParam(yaml_prefix + "min_height_checking/min_height", _min_height_);
+     435             : 
+     436          94 :   param_loader.loadParam(yaml_prefix + "require_gain_manager", _gain_manager_required_);
+     437          94 :   param_loader.loadParam(yaml_prefix + "require_constraint_manager", _constraint_manager_required_);
+     438             : 
+     439          94 :   param_loader.loadParam(yaml_prefix + "flight_timer/enabled", _flighttime_timer_enabled_);
+     440          94 :   param_loader.loadParam(yaml_prefix + "flight_timer/rate", _flighttime_timer_rate_);
+     441          94 :   param_loader.loadParam(yaml_prefix + "flight_timer/max_time", _flighttime_max_time_);
+     442             : 
+     443          94 :   param_loader.loadParam(yaml_prefix + "max_throttle/enabled", _maxthrottle_timer_enabled_);
+     444          94 :   param_loader.loadParam(yaml_prefix + "max_throttle/rate", _maxthrottle_timer_rate_);
+     445          94 :   param_loader.loadParam(yaml_prefix + "max_throttle/max_throttle", _maxthrottle_max_throttle_);
+     446          94 :   param_loader.loadParam(yaml_prefix + "max_throttle/eland_timeout", _maxthrottle_eland_timeout_);
+     447          94 :   param_loader.loadParam(yaml_prefix + "max_throttle/ungrip_timeout", _maxthrottle_ungrip_timeout_);
+     448             : 
+     449          94 :   param_loader.loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_timer_rate_);
+     450             : 
+     451             :   // | ------------------- scope timer logger ------------------- |
+     452             : 
+     453          94 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     454         282 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     455          94 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     456             : 
+     457          94 :   if (!param_loader.loadedSuccessfully()) {
+     458           0 :     ROS_ERROR("[UavManager]: Could not load all parameters!");
+     459           0 :     ros::shutdown();
+     460             :   }
+     461             : 
+     462             :   // | --------------------- tf transformer --------------------- |
+     463             : 
+     464          94 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+     465          94 :   transformer_->setDefaultPrefix(_uav_name_);
+     466          94 :   transformer_->retryLookupNewest(true);
+     467             : 
+     468             :   // | ----------------------- subscribers ---------------------- |
+     469             : 
+     470         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+     471          94 :   shopts.nh                 = nh_;
+     472          94 :   shopts.node_name          = "UavManager";
+     473          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     474          94 :   shopts.threadsafe         = true;
+     475          94 :   shopts.autostart          = true;
+     476          94 :   shopts.queue_size         = 10;
+     477          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     478             : 
+     479          94 :   sh_controller_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics>(shopts, "controller_diagnostics_in");
+     480          94 :   sh_odometry_               = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &UavManager::callbackOdometry, this);
+     481          94 :   sh_estimation_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "odometry_diagnostics_in");
+     482          94 :   sh_control_manager_diag_   = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     483          94 :   sh_mass_estimate_          = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "mass_estimate_in");
+     484          94 :   sh_throttle_               = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "throttle_in");
+     485          94 :   sh_height_                 = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_in");
+     486          94 :   sh_hw_api_status_          = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in");
+     487          94 :   sh_gains_diag_             = mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics>(shopts, "gain_manager_diagnostics_in");
+     488          94 :   sh_constraints_diag_       = mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics>(shopts, "constraint_manager_diagnostics_in");
+     489          94 :   sh_hw_api_gnss_            = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "hw_api_gnss_in", &UavManager::callbackHwApiGNSS, this);
+     490          94 :   sh_max_height_             = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_height_in");
+     491          94 :   sh_tracker_cmd_            = mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>(shopts, "tracker_cmd_in");
+     492             : 
+     493             :   // | ----------------------- publishers ----------------------- |
+     494             : 
+     495          94 :   ph_diag_ = mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     496             : 
+     497             :   // | --------------------- service servers -------------------- |
+     498             : 
+     499          94 :   service_server_takeoff_           = nh_.advertiseService("takeoff_in", &UavManager::callbackTakeoff, this);
+     500          94 :   service_server_land_              = nh_.advertiseService("land_in", &UavManager::callbackLand, this);
+     501          94 :   service_server_land_home_         = nh_.advertiseService("land_home_in", &UavManager::callbackLandHome, this);
+     502          94 :   service_server_land_there_        = nh_.advertiseService("land_there_in", &UavManager::callbackLandThere, this);
+     503          94 :   service_server_midair_activation_ = nh_.advertiseService("midair_activation_in", &UavManager::callbackMidairActivation, this);
+     504          94 :   service_server_min_height_check_  = nh_.advertiseService("enable_min_height_check_in", &UavManager::callbackMinHeightCheck, this);
+     505             : 
+     506             :   // | --------------------- service clients -------------------- |
+     507             : 
+     508          94 :   sch_takeoff_               = mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>(nh_, "takeoff_out");
+     509          94 :   sch_land_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "land_out");
+     510          94 :   sch_eland_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+     511          94 :   sch_ehover_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ehover_out");
+     512          94 :   sch_switch_tracker_        = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_tracker_out");
+     513          94 :   sch_switch_controller_     = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_controller_out");
+     514          94 :   sch_emergency_reference_   = mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>(nh_, "emergency_reference_out");
+     515          94 :   sch_control_callbacks_     = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "enable_callbacks_out");
+     516          94 :   sch_arm_                   = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "arm_out");
+     517          94 :   sch_odometry_callbacks_    = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+     518          94 :   sch_ungrip_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+     519          94 :   sch_toggle_control_output_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "toggle_control_output_out");
+     520          94 :   sch_offboard_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "offboard_out");
+     521             : 
+     522             :   // | ---------------------- state machine --------------------- |
+     523             : 
+     524          94 :   current_state_landing_ = IDLE_STATE;
+     525             : 
+     526             :   // | ------------------------ profiler ------------------------ |
+     527             : 
+     528          94 :   profiler_ = mrs_lib::Profiler(nh_, "UavManager", _profiler_enabled_);
+     529             : 
+     530             :   // | ------------------------- timers ------------------------- |
+     531             : 
+     532          94 :   timer_landing_           = nh_.createTimer(ros::Rate(_landing_timer_rate_), &UavManager::timerLanding, this, false, false);
+     533          94 :   timer_takeoff_           = nh_.createTimer(ros::Rate(_takeoff_timer_rate_), &UavManager::timerTakeoff, this, false, false);
+     534          94 :   timer_flighttime_        = nh_.createTimer(ros::Rate(_flighttime_timer_rate_), &UavManager::timerFlightTime, this, false, false);
+     535          94 :   timer_diagnostics_       = nh_.createTimer(ros::Rate(_diagnostics_timer_rate_), &UavManager::timerDiagnostics, this);
+     536          94 :   timer_midair_activation_ = nh_.createTimer(ros::Rate(_midair_activation_timer_rate_), &UavManager::timerMidairActivation, this, false, false);
+     537         188 :   timer_max_height_        = nh_.createTimer(ros::Rate(_max_height_checking_rate_), &UavManager::timerMaxHeight, this, false,
+     538         188 :                                       _max_height_enabled_ && hw_api_capabilities_.produces_distance_sensor);
+     539         188 :   timer_min_height_        = nh_.createTimer(ros::Rate(_min_height_checking_rate_), &UavManager::timerMinHeight, this, false,
+     540         188 :                                       min_height_check_ && hw_api_capabilities_.produces_distance_sensor);
+     541             : 
+     542          91 :   bool should_check_throttle = hw_api_capabilities_.accepts_actuator_cmd || hw_api_capabilities_.accepts_control_group_cmd ||
+     543         185 :                                hw_api_capabilities_.accepts_attitude_rate_cmd || hw_api_capabilities_.accepts_attitude_cmd;
+     544             : 
+     545             :   timer_maxthrottle_ =
+     546          94 :       nh_.createTimer(ros::Rate(_maxthrottle_timer_rate_), &UavManager::timerMaxthrottle, this, false, _maxthrottle_timer_enabled_ && should_check_throttle);
+     547             : 
+     548             :   // | ----------------------- finish init ---------------------- |
+     549             : 
+     550          94 :   is_initialized_ = true;
+     551             : 
+     552          94 :   ROS_INFO("[UavManager]: initialized");
+     553             : 
+     554          94 :   ROS_DEBUG("[UavManager]: debug output is enabled");
+     555          94 : }
+     556             : 
+     557             : //}
+     558             : 
+     559             : //}
+     560             : 
+     561             : // | ---------------------- state machine --------------------- |
+     562             : 
+     563             : /* //{ changeLandingState() */
+     564             : 
+     565          17 : void UavManager::changeLandingState(LandingStates_t new_state) {
+     566             : 
+     567          17 :   previous_state_landing_ = current_state_landing_;
+     568          17 :   current_state_landing_  = new_state;
+     569             : 
+     570          17 :   switch (current_state_landing_) {
+     571             : 
+     572           5 :     case LANDING_STATE: {
+     573             : 
+     574           5 :       if (sh_mass_estimate_.hasMsg() && (ros::Time::now() - sh_mass_estimate_.lastMsgTime()).toSec() < 1.0) {
+     575             : 
+     576             :         // copy member variables
+     577           5 :         auto mass_esimtate = sh_mass_estimate_.getMsg();
+     578             : 
+     579           5 :         landing_uav_mass_ = mass_esimtate->data;
+     580             :       }
+     581             : 
+     582           5 :       break;
+     583             :     };
+     584             : 
+     585          12 :     default: {
+     586          12 :       break;
+     587             :     }
+     588             :   }
+     589             : 
+     590             :   // just for ROS_INFO
+     591          17 :   ROS_INFO("[UavManager]: Switching landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+     592          17 : }
+     593             : 
+     594             : //}
+     595             : 
+     596             : // --------------------------------------------------------------
+     597             : // |                           timers                           |
+     598             : // --------------------------------------------------------------
+     599             : 
+     600             : /* timerHwApiCapabilities() //{ */
+     601             : 
+     602          98 : void UavManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+     603             : 
+     604         196 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", 1.0, 1.0, event);
+     605         196 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+     606             : 
+     607          98 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+     608           4 :     ROS_INFO_THROTTLE(1.0, "[UavManager]: waiting for HW API capabilities");
+     609           4 :     return;
+     610             :   }
+     611             : 
+     612          94 :   hw_api_capabilities_ = *sh_hw_api_capabilities_.getMsg();
+     613             : 
+     614          94 :   ROS_INFO("[UavManager]: got HW API capabilities, initializing");
+     615             : 
+     616          94 :   initialize();
+     617             : 
+     618          94 :   timer_hw_api_capabilities_.stop();
+     619             : }
+     620             : 
+     621             : //}
+     622             : 
+     623             : /* //{ timerLanding() */
+     624             : 
+     625         785 : void UavManager::timerLanding(const ros::TimerEvent& event) {
+     626             : 
+     627         785 :   if (!is_initialized_)
+     628           0 :     return;
+     629             : 
+     630        1570 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerLanding", _landing_timer_rate_, 0.1, event);
+     631        1570 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerLanding", scope_timer_logger_, scope_timer_enabled_);
+     632             : 
+     633         785 :   auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+     634             : 
+     635             :   // copy member variables
+     636         785 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     637         785 :   auto odometry                    = sh_odometry_.getMsg();
+     638         785 :   auto tracker_cmd                 = sh_tracker_cmd_.getMsg();
+     639             : 
+     640         785 :   std::optional<double> desired_throttle;
+     641             : 
+     642         785 :   if (sh_throttle_.hasMsg() && (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() < 1.0) {
+     643         785 :     desired_throttle = sh_throttle_.getMsg()->data;
+     644             :   }
+     645             : 
+     646         785 :   auto res = transformer_->transformSingle(land_there_reference, odometry->header.frame_id);
+     647             : 
+     648         785 :   mrs_msgs::ReferenceStamped land_there_current_frame;
+     649             : 
+     650         785 :   if (res) {
+     651         785 :     land_there_current_frame = res.value();
+     652             :   } else {
+     653             : 
+     654           0 :     ROS_ERROR("[UavManager]: could not transform the reference into the current frame! land by yourself pls.");
+     655           0 :     return;
+     656             :   }
+     657             : 
+     658         785 :   if (current_state_landing_ == IDLE_STATE) {
+     659             : 
+     660           0 :     return;
+     661             : 
+     662         785 :   } else if (current_state_landing_ == GOTO_STATE) {
+     663             : 
+     664         360 :     auto [pos_x, pos_y, pos_z] = mrs_lib::getPosition(*tracker_cmd);
+     665         360 :     auto [ref_x, ref_y, ref_z] = mrs_lib::getPosition(land_there_current_frame);
+     666             : 
+     667         360 :     double pos_heading = tracker_cmd->heading;
+     668             : 
+     669         360 :     double ref_heading = 0;
+     670             :     try {
+     671         360 :       ref_heading = mrs_lib::getHeading(land_there_current_frame);
+     672             :     }
+     673           0 :     catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     674           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     675           0 :       return;
+     676             :     }
+     677             : 
+     678         367 :     if (mrs_lib::geometry::dist(vec3_t(pos_x, pos_y, pos_z), vec3_t(ref_x, ref_y, ref_z)) < _landing_tracking_tolerance_translation_ &&
+     679           7 :         fabs(radians::diff(pos_heading, ref_heading)) < _landing_tracking_tolerance_heading_) {
+     680             : 
+     681          14 :       auto [success, message] = landWithDescendImpl();
+     682             : 
+     683           7 :       if (!success) {
+     684             : 
+     685           0 :         ROS_ERROR_THROTTLE(1.0, "[UavManager]: call for landing failed: '%s'", message.c_str());
+     686             :       }
+     687             : 
+     688         353 :     } else if (!control_manager_diagnostics->tracker_status.have_goal && control_manager_diagnostics->flying_normally) {
+     689             : 
+     690           1 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: the tracker does not have a goal while flying home, setting the reference again");
+     691             : 
+     692           2 :       mrs_msgs::ReferenceStamped reference_out;
+     693             : 
+     694             :       {
+     695           2 :         std::scoped_lock lock(mutex_land_there_reference_);
+     696             : 
+     697             :         // get the current altitude in land_there_reference_.header.frame_id;
+     698           2 :         geometry_msgs::PoseStamped current_pose;
+     699           1 :         current_pose.header.stamp     = ros::Time::now();
+     700           1 :         current_pose.header.frame_id  = _uav_name_ + "/fcu";
+     701           1 :         current_pose.pose.position.x  = 0;
+     702           1 :         current_pose.pose.position.y  = 0;
+     703           1 :         current_pose.pose.position.z  = 0;
+     704           1 :         current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     705             : 
+     706           1 :         auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+     707             : 
+     708           1 :         if (response) {
+     709             : 
+     710           1 :           land_there_reference_.reference.position.z = response.value().pose.position.z;
+     711           1 :           ROS_DEBUG("[UavManager]: current altitude is %.2f m", land_there_reference_.reference.position.z);
+     712             : 
+     713             :         } else {
+     714             : 
+     715           0 :           std::stringstream ss;
+     716           0 :           ss << "could not transform current height to " << land_there_reference_.header.frame_id;
+     717           0 :           ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+     718             :         }
+     719             : 
+     720           1 :         reference_out.header.frame_id = land_there_reference_.header.frame_id;
+     721           1 :         reference_out.header.stamp    = ros::Time::now();
+     722           1 :         reference_out.reference       = land_there_reference_.reference;
+     723             :       }
+     724             : 
+     725           1 :       emergencyReferenceSrv(reference_out);
+     726             :     }
+     727             : 
+     728         425 :   } else if (current_state_landing_ == LANDING_STATE) {
+     729             : 
+     730             :     // we should not attempt to finish the landing if some other tracked was activated
+     731         425 :     if (_landing_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+     732             : 
+     733         425 :       if (desired_throttle) {
+     734             : 
+     735             :         // recalculate the mass based on the throttle
+     736         425 :         throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(_throttle_model_, desired_throttle.value()) / _g_;
+     737         425 :         ROS_INFO_THROTTLE(1.0, "[UavManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+     738             : 
+     739             :         // condition for automatic motor turn off
+     740         425 :         if (((throttle_mass_estimate_ < _landing_cutoff_mass_factor_ * landing_uav_mass_) || desired_throttle < 0.01)) {
+     741             : 
+     742         110 :           if (!throttle_under_threshold_) {
+     743             : 
+     744           5 :             throttle_mass_estimate_first_time_ = ros::Time::now();
+     745           5 :             throttle_under_threshold_          = true;
+     746             :           }
+     747             : 
+     748         110 :           ROS_INFO_THROTTLE(0.5, "[UavManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+     749             : 
+     750             :         } else {
+     751             : 
+     752         315 :           throttle_under_threshold_ = false;
+     753             :         }
+     754             : 
+     755         425 :         if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _landing_cutoff_mass_timeout_)) {
+     756             : 
+     757           5 :           switchTrackerSrv(_null_tracker_name_);
+     758             : 
+     759           5 :           setControlCallbacksSrv(true);
+     760             : 
+     761           5 :           if (_landing_disarm_) {
+     762             : 
+     763           5 :             ROS_INFO("[UavManager]: disarming after landing");
+     764             : 
+     765           5 :             disarmSrv();
+     766             :           }
+     767             : 
+     768           5 :           changeLandingState(IDLE_STATE);
+     769             : 
+     770           5 :           ROS_INFO("[UavManager]: landing finished");
+     771             : 
+     772           5 :           timer_landing_.stop();
+     773             :         }
+     774             : 
+     775             :       } else {
+     776             : 
+     777           0 :         auto odometry = sh_odometry_.getMsg();
+     778             : 
+     779           0 :         double z_vel = odometry->twist.twist.linear.z;
+     780             : 
+     781           0 :         ROS_INFO_THROTTLE(1.0, "[UavManager]: landing: z-velocity: %.2f", z_vel);
+     782             : 
+     783             :         // condition for automatic motor turn off
+     784           0 :         if (z_vel > -0.1) {
+     785             : 
+     786           0 :           if (!velocity_under_threshold_) {
+     787             : 
+     788           0 :             velocity_under_threshold_first_time_ = ros::Time::now();
+     789           0 :             velocity_under_threshold_            = true;
+     790             :           }
+     791             : 
+     792           0 :           ROS_INFO_THROTTLE(0.5, "[UavManager]: velocity over threshold for %.2f s", (ros::Time::now() - velocity_under_threshold_first_time_).toSec());
+     793             : 
+     794             :         } else {
+     795             : 
+     796           0 :           velocity_under_threshold_ = false;
+     797             :         }
+     798             : 
+     799           0 :         if (velocity_under_threshold_ && ((ros::Time::now() - velocity_under_threshold_first_time_).toSec() > 3.0)) {
+     800             : 
+     801           0 :           switchTrackerSrv(_null_tracker_name_);
+     802             : 
+     803           0 :           setControlCallbacksSrv(true);
+     804             : 
+     805           0 :           if (_landing_disarm_) {
+     806             : 
+     807           0 :             ROS_INFO("[UavManager]: disarming after landing");
+     808             : 
+     809           0 :             disarmSrv();
+     810             :           }
+     811             : 
+     812           0 :           changeLandingState(IDLE_STATE);
+     813             : 
+     814           0 :           ROS_INFO("[UavManager]: landing finished");
+     815             : 
+     816           0 :           timer_landing_.stop();
+     817             :         }
+     818             :       }
+     819             : 
+     820             :     } else {
+     821             : 
+     822           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: incorrect tracker detected during landing!");
+     823             :     }
+     824             :   }
+     825             : }
+     826             : 
+     827             : //}
+     828             : 
+     829             : /* //{ timerTakeoff() */
+     830             : 
+     831         971 : void UavManager::timerTakeoff(const ros::TimerEvent& event) {
+     832             : 
+     833         971 :   if (!is_initialized_)
+     834           1 :     return;
+     835             : 
+     836        1942 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerTakeoff", _takeoff_timer_rate_, 0.1, event);
+     837        1942 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerTakeoff", scope_timer_logger_, scope_timer_enabled_);
+     838             : 
+     839         971 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     840             : 
+     841         971 :   if (waiting_for_takeoff_) {
+     842             : 
+     843          21 :     if (control_manager_diagnostics->active_tracker == _takeoff_tracker_name_ && control_manager_diagnostics->tracker_status.have_goal) {
+     844             : 
+     845          20 :       waiting_for_takeoff_ = false;
+     846             :     } else {
+     847             : 
+     848           1 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: waiting for takeoff confirmation from the ControlManager");
+     849           1 :       return;
+     850             :     }
+     851             :   }
+     852             : 
+     853         970 :   if (takingoff_) {
+     854             : 
+     855         970 :     if (control_manager_diagnostics->active_tracker != _takeoff_tracker_name_ || !control_manager_diagnostics->tracker_status.have_goal) {
+     856             : 
+     857          20 :       ROS_INFO("[UavManager]: take off finished, switching to %s", _after_takeoff_tracker_name_.c_str());
+     858             : 
+     859          20 :       switchTrackerSrv(_after_takeoff_tracker_name_);
+     860             : 
+     861          20 :       switchControllerSrv(_after_takeoff_controller_name_);
+     862             : 
+     863          20 :       setOdometryCallbacksSrv(true);
+     864             : 
+     865          20 :       timer_takeoff_.stop();
+     866             :     }
+     867             :   }
+     868             : }
+     869             : 
+     870             : //}
+     871             : 
+     872             : /* //{ timerMaxHeight() */
+     873             : 
+     874       20660 : void UavManager::timerMaxHeight(const ros::TimerEvent& event) {
+     875             : 
+     876       20660 :   if (!is_initialized_)
+     877       14133 :     return;
+     878             : 
+     879       41320 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxHeight", _max_height_checking_rate_, 0.1, event);
+     880       41320 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxHeight", scope_timer_logger_, scope_timer_enabled_);
+     881             : 
+     882       20660 :   if (!sh_max_height_.hasMsg() || !sh_height_.hasMsg() || !sh_odometry_.hasMsg()) {
+     883        7682 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: maxHeightTimer() not spinning, missing data");
+     884        7682 :     return;
+     885             :   }
+     886             : 
+     887       12978 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     888             : 
+     889       12978 :   if (!fixing_max_height_ && !control_manager_diag->flying_normally) {
+     890        6451 :     return;
+     891             :   }
+     892             : 
+     893        6527 :   auto   odometry = sh_odometry_.getMsg();
+     894        6527 :   double height   = sh_height_.getMsg()->value;
+     895             : 
+     896             :   // transform max z to the height frame
+     897        6527 :   geometry_msgs::PointStamped point;
+     898        6527 :   point.header  = sh_max_height_.getMsg()->header;
+     899        6527 :   point.point.z = sh_max_height_.getMsg()->value;
+     900             : 
+     901        6527 :   auto res = transformer_->transformSingle(point, sh_height_.getMsg()->header.frame_id);
+     902             : 
+     903             :   double max_z_in_height;
+     904             : 
+     905        6527 :   if (res) {
+     906        6527 :     max_z_in_height = res->point.z;
+     907             :   } else {
+     908           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: timerMaxHeight() not working, cannot transform max z to the height frame");
+     909           0 :     return;
+     910             :   }
+     911             : 
+     912        6527 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+     913             : 
+     914        6527 :   double odometry_heading = 0;
+     915             :   try {
+     916        6527 :     odometry_heading = mrs_lib::getHeading(odometry);
+     917             :   }
+     918           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     919           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     920           0 :     return;
+     921             :   }
+     922             : 
+     923        6527 :   if (height > max_z_in_height) {
+     924             : 
+     925          53 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: max height exceeded: %.2f >  %.2f, triggering safety goto", height, max_z_in_height);
+     926             : 
+     927         106 :     mrs_msgs::ReferenceStamped reference_out;
+     928          53 :     reference_out.header.frame_id = odometry->header.frame_id;
+     929          53 :     reference_out.header.stamp    = ros::Time::now();
+     930             : 
+     931          53 :     reference_out.reference.position.x = odometry_x;
+     932          53 :     reference_out.reference.position.y = odometry_y;
+     933          53 :     reference_out.reference.position.z = odometry_z + ((max_z_in_height - _max_height_offset_) - height);
+     934             : 
+     935          53 :     reference_out.reference.heading = odometry_heading;
+     936             : 
+     937          53 :     setControlCallbacksSrv(false);
+     938             : 
+     939          53 :     bool success = emergencyReferenceSrv(reference_out);
+     940             : 
+     941          53 :     if (success) {
+     942             : 
+     943          53 :       ROS_INFO("[UavManager]: descending");
+     944             : 
+     945          53 :       fixing_max_height_ = true;
+     946             : 
+     947             :     } else {
+     948             : 
+     949           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not descend");
+     950             : 
+     951           0 :       setControlCallbacksSrv(true);
+     952             :     }
+     953             :   }
+     954             : 
+     955        6527 :   if (fixing_max_height_ && height < max_z_in_height) {
+     956             : 
+     957           1 :     setControlCallbacksSrv(true);
+     958             : 
+     959           1 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+     960             : 
+     961           1 :     fixing_max_height_ = false;
+     962             :   }
+     963             : }
+     964             : 
+     965             : //}
+     966             : 
+     967             : /* //{ timerMinHeight() */
+     968             : 
+     969       20591 : void UavManager::timerMinHeight(const ros::TimerEvent& event) {
+     970             : 
+     971       20591 :   if (!is_initialized_)
+     972       14153 :     return;
+     973             : 
+     974       20591 :   ROS_INFO_ONCE("[UavManager]: min height timer spinning");
+     975             : 
+     976       41182 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMinHeight", _min_height_checking_rate_, 0.1, event);
+     977       41182 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMinHeight", scope_timer_logger_, scope_timer_enabled_);
+     978             : 
+     979       20591 :   if (!sh_odometry_.hasMsg() || !sh_height_.hasMsg() || !sh_control_manager_diag_.hasMsg()) {
+     980        7665 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: minHeightTimer() not spinning, missing data");
+     981        7665 :     return;
+     982             :   }
+     983             : 
+     984       12926 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     985             : 
+     986       12926 :   if (!fixing_min_height_ && !control_manager_diag->flying_normally) {
+     987        6488 :     return;
+     988             :   }
+     989             : 
+     990        6438 :   auto   odometry = sh_odometry_.getMsg();
+     991        6438 :   double height   = sh_height_.getMsg()->value;
+     992             : 
+     993        6438 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+     994             : 
+     995        6438 :   double odometry_heading = 0;
+     996             : 
+     997             :   try {
+     998        6438 :     odometry_heading = mrs_lib::getHeading(odometry);
+     999             :   }
+    1000           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    1001           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+    1002           0 :     return;
+    1003             :   }
+    1004             : 
+    1005        6438 :   if (height < _min_height_) {
+    1006             : 
+    1007          33 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: min height breached: %.2f < %.2f, triggering safety goto", height, _min_height_);
+    1008             : 
+    1009          66 :     mrs_msgs::ReferenceStamped reference_out;
+    1010          33 :     reference_out.header.frame_id = odometry->header.frame_id;
+    1011          33 :     reference_out.header.stamp    = ros::Time::now();
+    1012             : 
+    1013          33 :     reference_out.reference.position.x = odometry_x;
+    1014          33 :     reference_out.reference.position.y = odometry_y;
+    1015          33 :     reference_out.reference.position.z = odometry_z + ((_min_height_ + _min_height_offset_) - height);
+    1016             : 
+    1017          33 :     reference_out.reference.heading = odometry_heading;
+    1018             : 
+    1019          33 :     setControlCallbacksSrv(false);
+    1020             : 
+    1021          33 :     bool success = emergencyReferenceSrv(reference_out);
+    1022             : 
+    1023          33 :     if (success) {
+    1024             : 
+    1025          33 :       ROS_INFO("[UavManager]: ascending");
+    1026             : 
+    1027          33 :       fixing_min_height_ = true;
+    1028             : 
+    1029             :     } else {
+    1030             : 
+    1031           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not ascend");
+    1032             : 
+    1033           0 :       setControlCallbacksSrv(true);
+    1034             :     }
+    1035             :   }
+    1036             : 
+    1037        6438 :   if (fixing_min_height_ && height > _min_height_) {
+    1038             : 
+    1039           2 :     setControlCallbacksSrv(true);
+    1040             : 
+    1041           2 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+    1042             : 
+    1043           2 :     fixing_min_height_ = false;
+    1044             :   }
+    1045             : }
+    1046             : 
+    1047             : //}
+    1048             : 
+    1049             : /* //{ timerFlightTime() */
+    1050             : 
+    1051         283 : void UavManager::timerFlightTime(const ros::TimerEvent& event) {
+    1052             : 
+    1053         283 :   if (!is_initialized_)
+    1054           0 :     return;
+    1055             : 
+    1056         849 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFlightTime", _flighttime_timer_rate_, 0.1, event);
+    1057         849 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerFlightTime", scope_timer_logger_, scope_timer_enabled_);
+    1058             : 
+    1059         283 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1060             : 
+    1061         283 :   flighttime += 1.0 / _flighttime_timer_rate_;
+    1062             : 
+    1063         283 :   mrs_msgs::Float64 flight_time;
+    1064         283 :   flight_time.value = flighttime;
+    1065             : 
+    1066             :   // if enabled, start the timer for measuring the flight time
+    1067         283 :   if (_flighttime_timer_enabled_) {
+    1068             : 
+    1069           0 :     if (flighttime > _flighttime_max_time_) {
+    1070             : 
+    1071           0 :       flighttime = 0;
+    1072           0 :       timer_flighttime_.stop();
+    1073             : 
+    1074           0 :       ROS_INFO("[UavManager]: max flight time reached, landing");
+    1075             : 
+    1076           0 :       landImpl();
+    1077             :     }
+    1078             :   }
+    1079             : 
+    1080         283 :   mrs_lib::set_mutexed(mutex_flighttime_, flighttime, flighttime_);
+    1081             : }
+    1082             : 
+    1083             : //}
+    1084             : 
+    1085             : /* //{ timerMaxthrottle() */
+    1086             : 
+    1087       52060 : void UavManager::timerMaxthrottle(const ros::TimerEvent& event) {
+    1088             : 
+    1089       52060 :   if (!is_initialized_)
+    1090       15989 :     return;
+    1091             : 
+    1092       52060 :   if (!sh_throttle_.hasMsg() || (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() > 1.0) {
+    1093       15989 :     return;
+    1094             :   }
+    1095             : 
+    1096       36071 :   ROS_INFO_ONCE("[UavManager]: max throttle timer spinning");
+    1097             : 
+    1098      108213 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxthrottle", _maxthrottle_timer_rate_, 0.03, event);
+    1099      108213 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxthrottle", scope_timer_logger_, scope_timer_enabled_);
+    1100             : 
+    1101       36071 :   auto desired_throttle = sh_throttle_.getMsg()->data;
+    1102             : 
+    1103       36071 :   if (desired_throttle >= _maxthrottle_max_throttle_) {
+    1104             : 
+    1105          32 :     if (!maxthrottle_above_threshold_) {
+    1106             : 
+    1107           1 :       maxthrottle_first_time_      = ros::Time::now();
+    1108           1 :       maxthrottle_above_threshold_ = true;
+    1109           1 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: max throttle exceeded threshold (%.2f/%.2f)", desired_throttle, _maxthrottle_max_throttle_);
+    1110             : 
+    1111             :     } else {
+    1112             : 
+    1113          31 :       ROS_WARN_THROTTLE(0.1, "[UavManager]: throttle over threshold (%.2f/%.2f) for %.2f s", desired_throttle, _maxthrottle_max_throttle_,
+    1114             :                         (ros::Time::now() - maxthrottle_first_time_).toSec());
+    1115             :     }
+    1116             : 
+    1117             :   } else {
+    1118             : 
+    1119       36039 :     maxthrottle_above_threshold_ = false;
+    1120             :   }
+    1121             : 
+    1122       36071 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_ungrip_timeout_) {
+    1123             : 
+    1124          16 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, ungripping payload", desired_throttle,
+    1125             :                       _maxthrottle_max_throttle_, _maxthrottle_ungrip_timeout_);
+    1126             : 
+    1127          16 :     ungripSrv();
+    1128             :   }
+    1129             : 
+    1130       36071 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_eland_timeout_) {
+    1131             : 
+    1132           1 :     timer_maxthrottle_.stop();
+    1133             : 
+    1134           1 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, calling for emergency landing", desired_throttle,
+    1135             :                        _maxthrottle_max_throttle_, _maxthrottle_eland_timeout_);
+    1136             : 
+    1137           1 :     elandSrv();
+    1138             :   }
+    1139             : }
+    1140             : 
+    1141             : //}
+    1142             : 
+    1143             : /* //{ timerDiagnostics() */
+    1144             : 
+    1145        2055 : void UavManager::timerDiagnostics(const ros::TimerEvent& event) {
+    1146             : 
+    1147        2055 :   if (!is_initialized_)
+    1148           0 :     return;
+    1149             : 
+    1150        6165 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _maxthrottle_timer_rate_, 0.03, event);
+    1151        6165 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    1152             : 
+    1153        2055 :   bool got_gps_est = false;
+    1154        2055 :   bool got_rtk_est = false;
+    1155             : 
+    1156        2055 :   if (sh_estimation_diagnostics_.hasMsg()) {  // get current position in lat-lon
+    1157             : 
+    1158        3420 :     auto                     estimation_diag  = sh_estimation_diagnostics_.getMsg();
+    1159        1710 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    1160             : 
+    1161        2108 :     got_gps_est = std::find(state_estimators.begin(), state_estimators.end(), "gps_garmin") != state_estimators.end() ||
+    1162         398 :                   std::find(state_estimators.begin(), state_estimators.end(), "gps_baro") != state_estimators.end();
+    1163        1710 :     got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    1164             :   }
+    1165             : 
+    1166        4110 :   mrs_msgs::UavManagerDiagnostics diag;
+    1167             : 
+    1168        2055 :   diag.stamp    = ros::Time::now();
+    1169        2055 :   diag.uav_name = _uav_name_;
+    1170             : 
+    1171        2055 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1172             : 
+    1173             :   // fill in the acumulated flight time
+    1174        2055 :   diag.flight_time = flighttime;
+    1175             : 
+    1176        2055 :   if (sh_odometry_.hasMsg()) {  // get current position in lat-lon
+    1177             : 
+    1178        1717 :     if (got_gps_est || got_rtk_est) {
+    1179             : 
+    1180        3364 :       nav_msgs::Odometry odom = *sh_odometry_.getMsg();
+    1181             : 
+    1182        3364 :       geometry_msgs::PoseStamped uav_pose;
+    1183        1682 :       uav_pose.pose = mrs_lib::getPose(odom);
+    1184             : 
+    1185        5046 :       auto res = transformer_->transformSingle(uav_pose, "latlon_origin");
+    1186             : 
+    1187        1682 :       if (res) {
+    1188        1682 :         diag.cur_latitude  = res.value().pose.position.x;
+    1189        1682 :         diag.cur_longitude = res.value().pose.position.y;
+    1190             :       }
+    1191             :     }
+    1192             :   }
+    1193             : 
+    1194        2055 :   if (takeoff_successful_) {
+    1195             : 
+    1196         322 :     if (got_gps_est || got_rtk_est) {
+    1197             : 
+    1198         644 :       auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+    1199             : 
+    1200         966 :       auto res = transformer_->transformSingle(land_there_reference, "latlon_origin");
+    1201             : 
+    1202         322 :       if (res) {
+    1203         322 :         diag.home_latitude  = res.value().reference.position.x;
+    1204         322 :         diag.home_longitude = res.value().reference.position.y;
+    1205             :       }
+    1206             :     }
+    1207             :   }
+    1208             : 
+    1209        2055 :   ph_diag_.publish(diag);
+    1210             : }
+    1211             : 
+    1212             : //}
+    1213             : 
+    1214             : /* //{ timerMidairActivation() */
+    1215             : 
+    1216          66 : void UavManager::timerMidairActivation([[maybe_unused]] const ros::TimerEvent& event) {
+    1217             : 
+    1218          66 :   if (!is_initialized_)
+    1219           0 :     return;
+    1220             : 
+    1221          66 :   ROS_INFO_THROTTLE(0.1, "[UavManager]: waiting for OFFBOARD");
+    1222             : 
+    1223          66 :   if (sh_hw_api_status_.getMsg()->offboard) {
+    1224             : 
+    1225          66 :     ROS_INFO("[UavManager]: OFFBOARD detected");
+    1226             : 
+    1227          66 :     setOdometryCallbacksSrv(true);
+    1228             : 
+    1229             :     {
+    1230          66 :       bool controller_switched = switchControllerSrv(_midair_activation_after_controller_);
+    1231             : 
+    1232          66 :       if (!controller_switched) {
+    1233             : 
+    1234           1 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_controller_.c_str());
+    1235             : 
+    1236           1 :         ehoverSrv();
+    1237             : 
+    1238           1 :         timer_midair_activation_.stop();
+    1239             : 
+    1240           1 :         return;
+    1241             :       }
+    1242             :     }
+    1243             : 
+    1244             :     {
+    1245          65 :       bool tracker_switched = switchTrackerSrv(_midair_activation_after_tracker_);
+    1246             : 
+    1247          65 :       if (!tracker_switched) {
+    1248             : 
+    1249           1 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_tracker_.c_str());
+    1250             : 
+    1251           1 :         ehoverSrv();
+    1252             : 
+    1253           1 :         timer_midair_activation_.stop();
+    1254             : 
+    1255           1 :         return;
+    1256             :       }
+    1257             :     }
+    1258             : 
+    1259          64 :     timer_midair_activation_.stop();
+    1260             : 
+    1261          64 :     return;
+    1262             :   }
+    1263             : 
+    1264           0 :   if ((ros::Time::now() - midair_activation_started_).toSec() > 0.5) {
+    1265             : 
+    1266           0 :     ROS_ERROR("[UavManager]: waiting for OFFBOARD timeouted, reverting");
+    1267             : 
+    1268           0 :     toggleControlOutput(false);
+    1269             : 
+    1270           0 :     timer_midair_activation_.stop();
+    1271             : 
+    1272           0 :     return;
+    1273             :   }
+    1274             : }
+    1275             : 
+    1276             : //}
+    1277             : 
+    1278             : // --------------------------------------------------------------
+    1279             : // |                          callbacks                         |
+    1280             : // --------------------------------------------------------------
+    1281             : 
+    1282             : // | --------------------- topic callbacks -------------------- |
+    1283             : 
+    1284             : /* //{ callbackHwApiGNSS() */
+    1285             : 
+    1286       72444 : void UavManager::callbackHwApiGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    1287             : 
+    1288       72444 :   if (!is_initialized_)
+    1289          26 :     return;
+    1290             : 
+    1291      217254 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiGNSS");
+    1292      217254 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::callbackHwApiGNSS", scope_timer_logger_, scope_timer_enabled_);
+    1293             : 
+    1294       72418 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    1295             : }
+    1296             : 
+    1297             : //}
+    1298             : 
+    1299             : /* //{ callbackOdometry() */
+    1300             : 
+    1301      158554 : void UavManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    1302             : 
+    1303      158554 :   if (!is_initialized_)
+    1304           0 :     return;
+    1305             : 
+    1306      158554 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    1307             : }
+    1308             : 
+    1309             : //}
+    1310             : 
+    1311             : // | -------------------- service callbacks ------------------- |
+    1312             : 
+    1313             : /* //{ callbackTakeoff() */
+    1314             : 
+    1315          22 : bool UavManager::callbackTakeoff([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1316             : 
+    1317          22 :   if (!is_initialized_)
+    1318           0 :     return false;
+    1319             : 
+    1320          22 :   ROS_INFO("[UavManager]: takeoff called by service");
+    1321             : 
+    1322             :   /* preconditions //{ */
+    1323             : 
+    1324             :   {
+    1325          22 :     std::stringstream ss;
+    1326             : 
+    1327          22 :     if (!sh_odometry_.hasMsg()) {
+    1328           0 :       ss << "can not takeoff, missing odometry!";
+    1329           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1330           0 :       res.message = ss.str();
+    1331           0 :       res.success = false;
+    1332           0 :       return true;
+    1333             :     }
+    1334             : 
+    1335          22 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1336           0 :       ss << "can not takeoff, missing HW API status!";
+    1337           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1338           0 :       res.message = ss.str();
+    1339           0 :       res.success = false;
+    1340           0 :       return true;
+    1341             :     }
+    1342             : 
+    1343          22 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1344           0 :       ss << "can not takeoff, UAV not armed!";
+    1345           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1346           0 :       res.message = ss.str();
+    1347           0 :       res.success = false;
+    1348           0 :       return true;
+    1349             :     }
+    1350             : 
+    1351          22 :     if (!sh_hw_api_status_.getMsg()->offboard) {
+    1352           0 :       ss << "can not takeoff, UAV not in offboard mode!";
+    1353           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1354           0 :       res.message = ss.str();
+    1355           0 :       res.success = false;
+    1356           0 :       return true;
+    1357             :     }
+    1358             : 
+    1359             :     {
+    1360          22 :       if (!sh_control_manager_diag_.hasMsg() && (ros::Time::now() - sh_control_manager_diag_.lastMsgTime()).toSec() > 5.0) {
+    1361           0 :         ss << "can not takeoff, missing control manager diagnostics!";
+    1362           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1363           0 :         res.message = ss.str();
+    1364           0 :         res.success = false;
+    1365           0 :         return true;
+    1366             :       }
+    1367             : 
+    1368          22 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1369           0 :         ss << "can not takeoff, need '" << _null_tracker_name_ << "' to be active!";
+    1370           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1371           0 :         res.message = ss.str();
+    1372           0 :         res.success = false;
+    1373           0 :         return true;
+    1374             :       }
+    1375             :     }
+    1376             : 
+    1377          22 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1378           0 :       ss << "can not takeoff, missing controller diagnostics!";
+    1379           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1380           0 :       res.message = ss.str();
+    1381           0 :       res.success = false;
+    1382           0 :       return true;
+    1383             :     }
+    1384             : 
+    1385          22 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    1386           0 :       ss << "can not takeoff, GainManager is not running!";
+    1387           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1388           0 :       res.message = ss.str();
+    1389           0 :       res.success = false;
+    1390           0 :       return true;
+    1391             :     }
+    1392             : 
+    1393          22 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    1394           0 :       ss << "can not takeoff, ConstraintManager is not running!";
+    1395           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1396           0 :       res.message = ss.str();
+    1397           0 :       res.success = false;
+    1398           0 :       return true;
+    1399             :     }
+    1400             : 
+    1401          22 :     if (!sh_control_manager_diag_.getMsg()->output_enabled) {
+    1402             : 
+    1403           0 :       ss << "can not takeoff, Control Manager's output is disabled!";
+    1404           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1405           0 :       res.message = ss.str();
+    1406           0 :       res.success = false;
+    1407           0 :       return true;
+    1408             :     }
+    1409             : 
+    1410          22 :     if (number_of_takeoffs_ > 0) {
+    1411             : 
+    1412           0 :       auto last_mass_difference = mrs_lib::get_mutexed(mutex_last_mass_difference_, last_mass_difference_);
+    1413             : 
+    1414           0 :       if (last_mass_difference > 1.0) {
+    1415             : 
+    1416           0 :         ss << std::setprecision(2);
+    1417           0 :         ss << "can not takeoff, estimated mass difference is too large: " << _null_tracker_name_ << "!";
+    1418           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1419           0 :         res.message = ss.str();
+    1420           0 :         res.success = false;
+    1421           0 :         return true;
+    1422             :       }
+    1423             :     }
+    1424             :   }
+    1425             : 
+    1426             :   //}
+    1427             : 
+    1428          44 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+    1429          44 :   auto odometry                    = sh_odometry_.getMsg();
+    1430          22 :   auto [odom_x, odom_y, odom_z]    = mrs_lib::getPosition(sh_odometry_.getMsg());
+    1431             : 
+    1432             :   double odom_heading;
+    1433             :   try {
+    1434          22 :     odom_heading = mrs_lib::getHeading(sh_odometry_.getMsg());
+    1435             :   }
+    1436           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    1437           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+    1438             : 
+    1439           0 :     std::stringstream ss;
+    1440           0 :     ss << "could not calculate current heading";
+    1441           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1442           0 :     res.message = ss.str();
+    1443           0 :     res.success = false;
+    1444           0 :     return true;
+    1445             :   }
+    1446             : 
+    1447          22 :   ROS_INFO("[UavManager]: taking off");
+    1448             : 
+    1449          22 :   setOdometryCallbacksSrv(false);
+    1450             : 
+    1451             :   // activating the takeoff controller
+    1452             :   {
+    1453          22 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    1454          22 :     bool        controller_switched = switchControllerSrv(_takeoff_controller_name_);
+    1455             : 
+    1456             :     // if it fails, activate back the old controller
+    1457             :     // this is no big deal since the control outputs are not used
+    1458             :     // until NullTracker is active
+    1459          22 :     if (!controller_switched) {
+    1460             : 
+    1461           1 :       std::stringstream ss;
+    1462           1 :       ss << "could not activate '" << _takeoff_controller_name_ << "' for takeoff";
+    1463           1 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1464           1 :       res.success = false;
+    1465           1 :       res.message = ss.str();
+    1466             : 
+    1467           1 :       toggleControlOutput(false);
+    1468           1 :       disarmSrv();
+    1469             : 
+    1470           1 :       return true;
+    1471             :     }
+    1472             :   }
+    1473             : 
+    1474             :   // activate the takeoff tracker
+    1475             :   {
+    1476          21 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    1477          21 :     bool        tracker_switched = switchTrackerSrv(_takeoff_tracker_name_);
+    1478             : 
+    1479             :     // if it fails, activate back the old tracker
+    1480          21 :     if (!tracker_switched) {
+    1481             : 
+    1482           1 :       std::stringstream ss;
+    1483           1 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for takeoff";
+    1484           1 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1485           1 :       res.success = false;
+    1486           1 :       res.message = ss.str();
+    1487             : 
+    1488           1 :       toggleControlOutput(false);
+    1489           1 :       disarmSrv();
+    1490             : 
+    1491           1 :       return true;
+    1492             :     }
+    1493             :   }
+    1494             : 
+    1495             :   // let's sleep before calling take off.. the motors are rumping-up anyway
+    1496             :   // this solves on-time-happening race condition in the landoff tracker
+    1497          20 :   ros::Duration(0.3).sleep();
+    1498             : 
+    1499             :   // now the takeoff tracker and controller are active
+    1500             :   // the UAV is basically hovering on the ground
+    1501             :   // (the controller is probably rumping up the throttle now)
+    1502             : 
+    1503             :   // call the takeoff service at the takeoff tracker
+    1504             :   {
+    1505          20 :     bool takeoff_successful = takeoffSrv();
+    1506             : 
+    1507             :     // if the takeoff was not successful, switch to NullTracker
+    1508          20 :     if (takeoff_successful) {
+    1509             : 
+    1510             :       // save the current spot for later landing
+    1511             :       {
+    1512          20 :         std::scoped_lock lock(mutex_land_there_reference_);
+    1513             : 
+    1514          20 :         land_there_reference_.header               = odometry->header;
+    1515          20 :         land_there_reference_.reference.position.x = odom_x;
+    1516          20 :         land_there_reference_.reference.position.y = odom_y;
+    1517          20 :         land_there_reference_.reference.position.z = odom_z;
+    1518          20 :         land_there_reference_.reference.heading    = odom_heading;
+    1519             :       }
+    1520             : 
+    1521          20 :       timer_flighttime_.start();
+    1522             : 
+    1523          40 :       std::stringstream ss;
+    1524          20 :       ss << "taking off";
+    1525          20 :       res.success = true;
+    1526          20 :       res.message = ss.str();
+    1527          20 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1528             : 
+    1529          20 :       takingoff_ = true;
+    1530          20 :       number_of_takeoffs_++;
+    1531          20 :       waiting_for_takeoff_ = true;
+    1532             : 
+    1533             :       // start the takeoff timer
+    1534          20 :       timer_takeoff_.start();
+    1535             : 
+    1536          20 :       takeoff_successful_ = takeoff_successful;
+    1537             : 
+    1538             :     } else {
+    1539             : 
+    1540           0 :       std::stringstream ss;
+    1541           0 :       ss << "takeoff was not successful";
+    1542           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1543           0 :       res.success = false;
+    1544           0 :       res.message = ss.str();
+    1545             : 
+    1546             :       // if the call for takeoff fails, call for emergency landing
+    1547           0 :       elandSrv();
+    1548             :     }
+    1549             :   }
+    1550             : 
+    1551          20 :   return true;
+    1552             : }
+    1553             : 
+    1554             : //}
+    1555             : 
+    1556             : /* //{ callbackLand() */
+    1557             : 
+    1558           2 : bool UavManager::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1559             : 
+    1560           2 :   if (!is_initialized_)
+    1561           0 :     return false;
+    1562             : 
+    1563           2 :   ROS_INFO("[UavManager]: land called by service");
+    1564             : 
+    1565             :   /* preconditions //{ */
+    1566             : 
+    1567             :   {
+    1568           2 :     std::stringstream ss;
+    1569             : 
+    1570           2 :     if (!sh_odometry_.hasMsg()) {
+    1571           0 :       ss << "can not land, missing odometry!";
+    1572           0 :       res.message = ss.str();
+    1573           0 :       res.success = false;
+    1574           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1575           0 :       return true;
+    1576             :     }
+    1577             : 
+    1578             :     {
+    1579           2 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1580           0 :         ss << "can not land, missing control manager diagnostics!";
+    1581           0 :         res.message = ss.str();
+    1582           0 :         res.success = false;
+    1583           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1584           0 :         return true;
+    1585             :       }
+    1586             : 
+    1587           2 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1588           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1589           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1590           0 :         res.message = ss.str();
+    1591           0 :         res.success = false;
+    1592           0 :         return true;
+    1593             :       }
+    1594             :     }
+    1595             : 
+    1596           2 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1597           0 :       ss << "can not land, missing controller diagnostics!";
+    1598           0 :       res.message = ss.str();
+    1599           0 :       res.success = false;
+    1600           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1601           0 :       return true;
+    1602             :     }
+    1603             : 
+    1604           2 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1605           0 :       ss << "can not land, missing position cmd!";
+    1606           0 :       res.message = ss.str();
+    1607           0 :       res.success = false;
+    1608           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1609           0 :       return true;
+    1610             :     }
+    1611             :   }
+    1612             : 
+    1613             :   //}
+    1614             : 
+    1615           2 :   auto [success, message] = landWithDescendImpl();
+    1616             : 
+    1617           2 :   res.message = message;
+    1618           2 :   res.success = success;
+    1619             : 
+    1620           2 :   return true;
+    1621             : }
+    1622             : 
+    1623             : //}
+    1624             : 
+    1625             : /* //{ callbackLandHome() */
+    1626             : 
+    1627           2 : bool UavManager::callbackLandHome([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1628             : 
+    1629           2 :   if (!is_initialized_)
+    1630           0 :     return false;
+    1631             : 
+    1632           2 :   ROS_INFO("[UavManager]: land home called by service");
+    1633             : 
+    1634             :   /* preconditions //{ */
+    1635             : 
+    1636             :   {
+    1637           2 :     std::stringstream ss;
+    1638             : 
+    1639           2 :     if (number_of_takeoffs_ == 0) {
+    1640           0 :       ss << "can not land home, did not takeoff before!";
+    1641           0 :       res.message = ss.str();
+    1642           0 :       res.success = false;
+    1643           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1644           0 :       return true;
+    1645             :     }
+    1646             : 
+    1647           2 :     if (!sh_odometry_.hasMsg()) {
+    1648           0 :       ss << "can not land, missing odometry!";
+    1649           0 :       res.message = ss.str();
+    1650           0 :       res.success = false;
+    1651           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1652           0 :       return true;
+    1653             :     }
+    1654             : 
+    1655             :     {
+    1656           2 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1657           0 :         ss << "can not land, missing tracker status!";
+    1658           0 :         res.message = ss.str();
+    1659           0 :         res.success = false;
+    1660           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1661           0 :         return true;
+    1662             :       }
+    1663             : 
+    1664           2 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1665           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1666           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1667           0 :         res.message = ss.str();
+    1668           0 :         res.success = false;
+    1669           0 :         return true;
+    1670             :       }
+    1671             :     }
+    1672             : 
+    1673           2 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1674           0 :       ss << "can not land, missing controller diagnostics command!";
+    1675           0 :       res.message = ss.str();
+    1676           0 :       res.success = false;
+    1677           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1678           0 :       return true;
+    1679             :     }
+    1680             : 
+    1681           2 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1682           0 :       ss << "can not land, missing position cmd!";
+    1683           0 :       res.message = ss.str();
+    1684           0 :       res.success = false;
+    1685           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1686           0 :       return true;
+    1687             :     }
+    1688             : 
+    1689           2 :     if (fixing_max_height_) {
+    1690           0 :       ss << "can not land, descedning to safe height!";
+    1691           0 :       res.message = ss.str();
+    1692           0 :       res.success = false;
+    1693           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1694           0 :       return true;
+    1695             :     }
+    1696             : 
+    1697           2 :     if (current_state_landing_ != IDLE_STATE) {
+    1698           0 :       ss << "can not land, already landing!";
+    1699           0 :       res.message = ss.str();
+    1700           0 :       res.success = false;
+    1701           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1702           0 :       return true;
+    1703             :     }
+    1704             :   }
+    1705             : 
+    1706             :   //}
+    1707             : 
+    1708           2 :   ungripSrv();
+    1709             : 
+    1710           4 :   mrs_msgs::ReferenceStamped reference_out;
+    1711             : 
+    1712             :   {
+    1713           2 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1714             : 
+    1715             :     // get the current altitude in land_there_reference_.header.frame_id;
+    1716           2 :     geometry_msgs::PoseStamped current_pose;
+    1717           2 :     current_pose.header.stamp     = ros::Time::now();
+    1718           2 :     current_pose.header.frame_id  = _uav_name_ + "/fcu";
+    1719           2 :     current_pose.pose.position.x  = 0;
+    1720           2 :     current_pose.pose.position.y  = 0;
+    1721           2 :     current_pose.pose.position.z  = 0;
+    1722           2 :     current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1723             : 
+    1724           2 :     auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+    1725             : 
+    1726           2 :     if (response) {
+    1727             : 
+    1728           2 :       land_there_reference_.reference.position.z = response.value().pose.position.z;
+    1729           2 :       ROS_DEBUG("[UavManager]: current altitude is %.2f m", land_there_reference_.reference.position.z);
+    1730             : 
+    1731             :     } else {
+    1732             : 
+    1733           0 :       std::stringstream ss;
+    1734           0 :       ss << "could not transform current height to " << land_there_reference_.header.frame_id;
+    1735           0 :       ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1736             : 
+    1737           0 :       res.success = false;
+    1738           0 :       res.message = ss.str();
+    1739           0 :       return true;
+    1740             :     }
+    1741             : 
+    1742           2 :     reference_out.header.frame_id = land_there_reference_.header.frame_id;
+    1743           2 :     reference_out.header.stamp    = ros::Time::now();
+    1744           2 :     reference_out.reference       = land_there_reference_.reference;
+    1745             : 
+    1746           2 :     land_there_reference_ = reference_out;
+    1747             :   }
+    1748             : 
+    1749           2 :   bool service_success = emergencyReferenceSrv(reference_out);
+    1750             : 
+    1751           2 :   if (service_success) {
+    1752             : 
+    1753           4 :     std::stringstream ss;
+    1754           2 :     ss << "flying home for landing";
+    1755           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1756             : 
+    1757           2 :     res.success = true;
+    1758           2 :     res.message = ss.str();
+    1759             : 
+    1760             :     // stop the eventual takeoff
+    1761           2 :     waiting_for_takeoff_ = false;
+    1762           2 :     takingoff_           = false;
+    1763           2 :     timer_takeoff_.stop();
+    1764             : 
+    1765           2 :     throttle_under_threshold_          = false;
+    1766           2 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1767             : 
+    1768           2 :     changeLandingState(GOTO_STATE);
+    1769             : 
+    1770           2 :     timer_landing_.start();
+    1771             : 
+    1772             :   } else {
+    1773             : 
+    1774           0 :     std::stringstream ss;
+    1775           0 :     ss << "can not fly home for landing";
+    1776           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1777             : 
+    1778           0 :     res.success = false;
+    1779           0 :     res.message = ss.str();
+    1780             :   }
+    1781             : 
+    1782           2 :   return true;
+    1783             : }
+    1784             : 
+    1785             : //}
+    1786             : 
+    1787             : /* //{ callbackLandThere() */
+    1788             : 
+    1789           1 : bool UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    1790             : 
+    1791           1 :   if (!is_initialized_)
+    1792           0 :     return false;
+    1793             : 
+    1794           1 :   ROS_INFO("[UavManager]: land there called by service");
+    1795             : 
+    1796             :   /* preconditions //{ */
+    1797             : 
+    1798             :   {
+    1799           1 :     std::stringstream ss;
+    1800             : 
+    1801           1 :     if (!sh_odometry_.hasMsg()) {
+    1802           0 :       ss << "can not land, missing odometry!";
+    1803           0 :       res.message = ss.str();
+    1804           0 :       res.success = false;
+    1805           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1806           0 :       return true;
+    1807             :     }
+    1808             : 
+    1809             :     {
+    1810           1 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1811           0 :         ss << "can not land, missing tracker status!";
+    1812           0 :         res.message = ss.str();
+    1813           0 :         res.success = false;
+    1814           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1815           0 :         return true;
+    1816             :       }
+    1817             : 
+    1818           1 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1819           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1820           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1821           0 :         res.message = ss.str();
+    1822           0 :         res.success = false;
+    1823           0 :         return true;
+    1824             :       }
+    1825             :     }
+    1826             : 
+    1827           1 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1828           0 :       ss << "can not land, missing controller diagnostics!";
+    1829           0 :       res.message = ss.str();
+    1830           0 :       res.success = false;
+    1831           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1832           0 :       return true;
+    1833             :     }
+    1834             : 
+    1835           1 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1836           0 :       ss << "can not land, missing position cmd!";
+    1837           0 :       res.message = ss.str();
+    1838           0 :       res.success = false;
+    1839           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1840           0 :       return true;
+    1841             :     }
+    1842             : 
+    1843           1 :     if (fixing_max_height_) {
+    1844           0 :       ss << "can not land, descedning to safe height!";
+    1845           0 :       res.message = ss.str();
+    1846           0 :       res.success = false;
+    1847           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1848           0 :       return true;
+    1849             :     }
+    1850             :   }
+    1851             : 
+    1852             :   //}
+    1853             : 
+    1854           1 :   ungripSrv();
+    1855             : 
+    1856           2 :   auto odometry = sh_odometry_.getMsg();
+    1857             : 
+    1858             :   // | ------ transform the reference to the current frame ------ |
+    1859             : 
+    1860           2 :   mrs_msgs::ReferenceStamped reference_in;
+    1861           1 :   reference_in.header    = req.header;
+    1862           1 :   reference_in.reference = req.reference;
+    1863             : 
+    1864           2 :   auto result = transformer_->transformSingle(reference_in, odometry->header.frame_id);
+    1865             : 
+    1866           1 :   if (!result) {
+    1867           0 :     std::stringstream ss;
+    1868           0 :     ss << "can not transform the reference to the current control frame!";
+    1869           0 :     res.message = ss.str();
+    1870           0 :     res.success = false;
+    1871           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1872           0 :     return true;
+    1873             :   }
+    1874             : 
+    1875             :   {
+    1876           1 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1877             : 
+    1878           1 :     land_there_reference_.header               = odometry->header;
+    1879           1 :     land_there_reference_.reference            = reference_in.reference;
+    1880           1 :     land_there_reference_.reference.position.z = odometry->pose.pose.position.z;
+    1881             :   }
+    1882             : 
+    1883           1 :   bool service_success = emergencyReferenceSrv(land_there_reference_);
+    1884             : 
+    1885           1 :   if (service_success) {
+    1886             : 
+    1887           2 :     std::stringstream ss;
+    1888           1 :     ss << "flying there for landing";
+    1889           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1890             : 
+    1891           1 :     res.success = true;
+    1892           1 :     res.message = ss.str();
+    1893             : 
+    1894             :     // stop the eventual takeoff
+    1895           1 :     waiting_for_takeoff_ = false;
+    1896           1 :     takingoff_           = false;
+    1897           1 :     timer_takeoff_.stop();
+    1898             : 
+    1899           1 :     throttle_under_threshold_          = false;
+    1900           1 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1901             : 
+    1902           1 :     changeLandingState(GOTO_STATE);
+    1903             : 
+    1904           1 :     timer_landing_.start();
+    1905             : 
+    1906             :   } else {
+    1907             : 
+    1908           0 :     std::stringstream ss;
+    1909           0 :     ss << "can not fly there for landing";
+    1910           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1911             : 
+    1912           0 :     res.success = false;
+    1913           0 :     res.message = ss.str();
+    1914             :   }
+    1915             : 
+    1916           1 :   return true;
+    1917             : }
+    1918             : 
+    1919             : //}
+    1920             : 
+    1921             : /* //{ callbackMidairActivation() */
+    1922             : 
+    1923          68 : bool UavManager::callbackMidairActivation([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1924             : 
+    1925          68 :   if (!is_initialized_)
+    1926           0 :     return false;
+    1927             : 
+    1928          68 :   ROS_INFO("[UavManager]: midair activation called by service");
+    1929             : 
+    1930             :   /* preconditions //{ */
+    1931             : 
+    1932             :   {
+    1933          68 :     std::stringstream ss;
+    1934             : 
+    1935          68 :     if (!sh_odometry_.hasMsg()) {
+    1936           0 :       ss << "can not activate, missing odometry!";
+    1937           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1938           0 :       res.message = ss.str();
+    1939           0 :       res.success = false;
+    1940           0 :       return true;
+    1941             :     }
+    1942             : 
+    1943          68 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1944           0 :       ss << "can not activate, missing HW API status!";
+    1945           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1946           0 :       res.message = ss.str();
+    1947           0 :       res.success = false;
+    1948           0 :       return true;
+    1949             :     }
+    1950             : 
+    1951          68 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1952           0 :       ss << "can not activate, UAV not armed!";
+    1953           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1954           0 :       res.message = ss.str();
+    1955           0 :       res.success = false;
+    1956           0 :       return true;
+    1957             :     }
+    1958             : 
+    1959          68 :     if (sh_hw_api_status_.getMsg()->offboard) {
+    1960           0 :       ss << "can not activate, UAV already in offboard mode!";
+    1961           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1962           0 :       res.message = ss.str();
+    1963           0 :       res.success = false;
+    1964           0 :       return true;
+    1965             :     }
+    1966             : 
+    1967             :     {
+    1968          68 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1969           0 :         ss << "can not activate, missing control manager diagnostics!";
+    1970           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1971           0 :         res.message = ss.str();
+    1972           0 :         res.success = false;
+    1973           0 :         return true;
+    1974             :       }
+    1975             : 
+    1976          68 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1977           0 :         ss << "can not activate, need '" << _null_tracker_name_ << "' to be active!";
+    1978           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1979           0 :         res.message = ss.str();
+    1980           0 :         res.success = false;
+    1981           0 :         return true;
+    1982             :       }
+    1983             :     }
+    1984             : 
+    1985          68 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1986           0 :       ss << "can not activate, missing controller diagnostics!";
+    1987           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1988           0 :       res.message = ss.str();
+    1989           0 :       res.success = false;
+    1990           0 :       return true;
+    1991             :     }
+    1992             : 
+    1993          68 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    1994           0 :       ss << "can not activate, GainManager is not running!";
+    1995           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1996           0 :       res.message = ss.str();
+    1997           0 :       res.success = false;
+    1998           0 :       return true;
+    1999             :     }
+    2000             : 
+    2001          68 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    2002           0 :       ss << "can not activate, ConstraintManager is not running!";
+    2003           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2004           0 :       res.message = ss.str();
+    2005           0 :       res.success = false;
+    2006           0 :       return true;
+    2007             :     }
+    2008             : 
+    2009          68 :     if (number_of_takeoffs_ > 0) {
+    2010           0 :       ss << "can not activate, we flew already!";
+    2011           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2012           0 :       res.message = ss.str();
+    2013           0 :       res.success = false;
+    2014           0 :       return true;
+    2015             :     }
+    2016             :   }
+    2017             : 
+    2018             :   //}
+    2019             : 
+    2020          68 :   auto [success, message] = midairActivationImpl();
+    2021             : 
+    2022          68 :   res.message = message;
+    2023          68 :   res.success = success;
+    2024             : 
+    2025          68 :   return true;
+    2026             : }
+    2027             : 
+    2028             : //}
+    2029             : 
+    2030             : /* //{ callbackMinHeightCheck() */
+    2031             : 
+    2032           2 : bool UavManager::callbackMinHeightCheck([[maybe_unused]] std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    2033             : 
+    2034           2 :   if (!is_initialized_)
+    2035           0 :     return false;
+    2036             : 
+    2037           2 :   min_height_check_ = req.data;
+    2038             : 
+    2039           2 :   std::stringstream ss;
+    2040             : 
+    2041           2 :   ss << "min height check " << (min_height_check_ ? "enabled" : "disabled");
+    2042             : 
+    2043           2 :   if (min_height_check_) {
+    2044           1 :     timer_min_height_.start();
+    2045             :   } else {
+    2046           1 :     timer_min_height_.stop();
+    2047             :   }
+    2048             : 
+    2049           2 :   ROS_INFO_STREAM("[UavManager]: " << ss.str());
+    2050             : 
+    2051           2 :   res.message = ss.str();
+    2052           2 :   res.success = true;
+    2053             : 
+    2054           2 :   return true;
+    2055             : }
+    2056             : 
+    2057             : //}
+    2058             : 
+    2059             : // | ------------------------ routines ------------------------ |
+    2060             : 
+    2061             : /* landImpl() //{ */
+    2062             : 
+    2063           5 : std::tuple<bool, std::string> UavManager::landImpl(void) {
+    2064             : 
+    2065             :   // activating the landing controller
+    2066             :   {
+    2067           5 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    2068           5 :     bool        controller_switched = switchControllerSrv(_landing_controller_name_);
+    2069             : 
+    2070             :     // if it fails, activate eland
+    2071             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2072             :     // just throw out error.
+    2073           5 :     if (!controller_switched) {
+    2074             : 
+    2075           0 :       std::stringstream ss;
+    2076           0 :       ss << "could not activate '" << _takeoff_controller_name_ << "' for landing";
+    2077           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2078             : 
+    2079           0 :       elandSrv();
+    2080             : 
+    2081           0 :       return std::tuple(false, ss.str());
+    2082             :     }
+    2083             :   }
+    2084             : 
+    2085             :   // activate the landing tracker
+    2086             :   {
+    2087           5 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    2088           5 :     bool        tracker_switched = switchTrackerSrv(_landing_tracker_name_);
+    2089             : 
+    2090             :     // if it fails, activate eland
+    2091             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2092             :     // just throw out error.
+    2093           5 :     if (!tracker_switched) {
+    2094             : 
+    2095           0 :       std::stringstream ss;
+    2096           0 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for landing";
+    2097           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2098             : 
+    2099           0 :       elandSrv();
+    2100             : 
+    2101           0 :       return std::tuple(false, ss.str());
+    2102             :     }
+    2103             :   }
+    2104             : 
+    2105             :   // call the landing service
+    2106             :   {
+    2107           5 :     bool land_successful = landSrv();
+    2108             : 
+    2109           5 :     if (land_successful) {
+    2110             : 
+    2111             :       // stop the eventual takeoff
+    2112           5 :       waiting_for_takeoff_ = false;
+    2113           5 :       takingoff_           = false;
+    2114           5 :       timer_takeoff_.stop();
+    2115             : 
+    2116             :       // stop counting the flight time
+    2117           5 :       timer_flighttime_.stop();
+    2118             : 
+    2119          10 :       auto controller_diagnostics = sh_controller_diagnostics_.getMsg();
+    2120             : 
+    2121             :       // remember the last valid mass estimated
+    2122             :       // used during subsequent takeoff
+    2123           5 :       if (controller_diagnostics->mass_estimator) {
+    2124           5 :         last_mass_difference_ = controller_diagnostics->mass_difference;
+    2125             :       }
+    2126             : 
+    2127           5 :       setOdometryCallbacksSrv(false);
+    2128             : 
+    2129           5 :       changeLandingState(LANDING_STATE);
+    2130             : 
+    2131           5 :       throttle_under_threshold_          = false;
+    2132           5 :       throttle_mass_estimate_first_time_ = ros::Time(0);
+    2133             : 
+    2134           5 :       timer_landing_.start();
+    2135             : 
+    2136           5 :       std::stringstream ss;
+    2137           5 :       ss << "landing initiated";
+    2138           5 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2139             : 
+    2140           5 :       return std::tuple(true, ss.str());
+    2141             : 
+    2142             :     } else {
+    2143             : 
+    2144           0 :       std::stringstream ss;
+    2145           0 :       ss << "could not land";
+    2146           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2147             : 
+    2148           0 :       elandSrv();
+    2149             : 
+    2150           0 :       return std::tuple(false, ss.str());
+    2151             :     }
+    2152             :   }
+    2153             : }
+    2154             : 
+    2155             : //}
+    2156             : 
+    2157             : /* landWithDescendImpl() //{ */
+    2158             : 
+    2159           9 : std::tuple<bool, std::string> UavManager::landWithDescendImpl(void) {
+    2160             : 
+    2161             :   // if the height information is available
+    2162           9 :   if (sh_height_.hasMsg()) {
+    2163             : 
+    2164           9 :     double height = sh_height_.getMsg()->value;
+    2165             : 
+    2166           9 :     if (height > 0 && height >= _landing_descend_height_ + 1.0) {
+    2167             : 
+    2168           4 :       auto odometry = sh_odometry_.getMsg();
+    2169             : 
+    2170           4 :       ungripSrv();
+    2171             : 
+    2172             :       {
+    2173           4 :         std::scoped_lock lock(mutex_land_there_reference_);
+    2174             : 
+    2175             :         // FOR FUTURE ME: Do not change this, we need it to be filled for the final check later
+    2176           4 :         land_there_reference_.header.frame_id      = "";
+    2177           4 :         land_there_reference_.header.stamp         = ros::Time::now();
+    2178           4 :         land_there_reference_.reference.position.x = odometry->pose.pose.position.x;
+    2179           4 :         land_there_reference_.reference.position.y = odometry->pose.pose.position.y;
+    2180           4 :         land_there_reference_.reference.position.z = odometry->pose.pose.position.z - (height - _landing_descend_height_);
+    2181           4 :         land_there_reference_.reference.heading    = mrs_lib::AttitudeConverter(odometry->pose.pose.orientation).getHeading();
+    2182             :       }
+    2183             : 
+    2184           4 :       bool service_success = emergencyReferenceSrv(land_there_reference_);
+    2185             : 
+    2186           4 :       if (service_success) {
+    2187             : 
+    2188           4 :         std::stringstream ss;
+    2189           4 :         ss << "flying down for landing";
+    2190           4 :         ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2191             : 
+    2192             :         // stop the eventual takeoff
+    2193           4 :         waiting_for_takeoff_ = false;
+    2194           4 :         takingoff_           = false;
+    2195           4 :         timer_takeoff_.stop();
+    2196             : 
+    2197           4 :         changeLandingState(GOTO_STATE);
+    2198             : 
+    2199           4 :         throttle_under_threshold_          = false;
+    2200           4 :         throttle_mass_estimate_first_time_ = ros::Time(0);
+    2201             : 
+    2202           4 :         timer_landing_.start();
+    2203             : 
+    2204           4 :         return std::tuple(true, ss.str());
+    2205             : 
+    2206             :       } else {
+    2207             : 
+    2208           0 :         std::stringstream ss;
+    2209           0 :         ss << "can not fly down for landing";
+    2210           0 :         ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    2211             :       }
+    2212             :     }
+    2213             :   }
+    2214             : 
+    2215          10 :   auto [success, message] = landImpl();
+    2216             : 
+    2217           5 :   return std::tuple(success, message);
+    2218             : }
+    2219             : 
+    2220             : //}
+    2221             : 
+    2222             : /* midairActivationImpl() //{ */
+    2223             : 
+    2224          68 : std::tuple<bool, std::string> UavManager::midairActivationImpl(void) {
+    2225             : 
+    2226             :   // 1. activate the mid-air activation controller
+    2227             :   // the controller will output hover-like control output
+    2228         136 :   std::string old_controller;
+    2229             :   {
+    2230          68 :     old_controller           = sh_control_manager_diag_.getMsg()->active_controller;
+    2231          68 :     bool controller_switched = switchControllerSrv(_midair_activation_during_controller_);
+    2232             : 
+    2233          68 :     if (!controller_switched) {
+    2234             : 
+    2235           0 :       std::stringstream ss;
+    2236           0 :       ss << "could not activate '" << _midair_activation_during_controller_ << "' for midair activation";
+    2237           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2238             : 
+    2239           0 :       return std::tuple(false, ss.str());
+    2240             :     }
+    2241             :   }
+    2242             : 
+    2243             :   // 2. turn Control Manager's output ON
+    2244             :   {
+    2245          68 :     bool output_enabled = toggleControlOutput(true);
+    2246             : 
+    2247          68 :     if (!output_enabled) {
+    2248             : 
+    2249           1 :       switchControllerSrv(old_controller);
+    2250             : 
+    2251           1 :       std::stringstream ss;
+    2252           1 :       ss << "could not enable Control Manager's output";
+    2253           1 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2254             : 
+    2255           1 :       return std::tuple(false, ss.str());
+    2256             :     }
+    2257             :   }
+    2258             : 
+    2259             :   // 3. activate the mid-air activation tracker
+    2260             :   // this will cause the Control Manager to output something else than min-throttle
+    2261         134 :   std::string old_tracker;
+    2262             :   {
+    2263          67 :     old_tracker = sh_control_manager_diag_.getMsg()->active_tracker;
+    2264             : 
+    2265          67 :     bool tracker_switched = switchTrackerSrv(_midair_activation_during_tracker_);
+    2266             : 
+    2267          67 :     if (!tracker_switched) {
+    2268             : 
+    2269           0 :       switchControllerSrv(old_controller);
+    2270           0 :       toggleControlOutput(false);
+    2271             : 
+    2272           0 :       std::stringstream ss;
+    2273           0 :       ss << "could not activate '" << _midair_activation_during_tracker_ << "' for midair activation";
+    2274           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2275             : 
+    2276           0 :       return std::tuple(false, ss.str());
+    2277             :     }
+    2278             :   }
+    2279             : 
+    2280             :   // 4. wait for 50 ms, that should be enough for the Pixhawk to start getting data
+    2281          67 :   ros::Duration(0.05).sleep();
+    2282             : 
+    2283             :   // 5. turn on the OFFBOARD MODE
+    2284             :   // since now, the UAV should be under our control
+    2285             :   {
+    2286          67 :     bool offboard_set = offboardSrv(true);
+    2287             : 
+    2288          67 :     if (!offboard_set) {
+    2289             : 
+    2290           1 :       switchTrackerSrv(old_tracker);
+    2291           1 :       switchControllerSrv(old_controller);
+    2292           1 :       toggleControlOutput(false);
+    2293             : 
+    2294           1 :       std::stringstream ss;
+    2295           1 :       ss << "could not activate offboard mode";
+    2296           1 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2297             : 
+    2298           1 :       return std::tuple(false, ss.str());
+    2299             :     }
+    2300             :   }
+    2301             : 
+    2302             :   // remember this time, later check for timeout
+    2303          66 :   midair_activation_started_ = ros::Time::now();
+    2304             : 
+    2305             :   // start the timer which should check if the offboard is on, activate proper controller and tracker or timeout
+    2306          66 :   timer_midair_activation_.start();
+    2307             : 
+    2308          66 :   std::stringstream ss;
+    2309          66 :   ss << "midair activation initiated, starting the timer";
+    2310          66 :   ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2311             : 
+    2312          66 :   return std::tuple(true, ss.str());
+    2313             : }
+    2314             : 
+    2315             : //}
+    2316             : 
+    2317             : // | ----------------- service client wrappers ---------------- |
+    2318             : 
+    2319             : /* setOdometryCallbacksSrv() //{ */
+    2320             : 
+    2321         113 : void UavManager::setOdometryCallbacksSrv(const bool& input) {
+    2322             : 
+    2323         113 :   ROS_INFO("[UavManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    2324             : 
+    2325         226 :   std_srvs::SetBool srv;
+    2326             : 
+    2327         113 :   srv.request.data = input;
+    2328             : 
+    2329         113 :   bool res = sch_odometry_callbacks_.call(srv);
+    2330             : 
+    2331         113 :   if (res) {
+    2332             : 
+    2333         113 :     if (!srv.response.success) {
+    2334           0 :       ROS_WARN("[UavManager]: service call for toggle odometry callbacks returned: %s.", srv.response.message.c_str());
+    2335             :     }
+    2336             : 
+    2337             :   } else {
+    2338           0 :     ROS_ERROR("[UavManager]: service call for toggle odometry callbacks failed!");
+    2339             :   }
+    2340         113 : }
+    2341             : 
+    2342             : //}
+    2343             : 
+    2344             : /* setControlCallbacksSrv() //{ */
+    2345             : 
+    2346          94 : void UavManager::setControlCallbacksSrv(const bool& input) {
+    2347             : 
+    2348          94 :   ROS_INFO("[UavManager]: switching control callbacks to %s", input ? "ON" : "OFF");
+    2349             : 
+    2350         188 :   std_srvs::SetBool srv;
+    2351             : 
+    2352          94 :   srv.request.data = input;
+    2353             : 
+    2354          94 :   bool res = sch_control_callbacks_.call(srv);
+    2355             : 
+    2356          94 :   if (res) {
+    2357             : 
+    2358          94 :     if (!srv.response.success) {
+    2359           0 :       ROS_WARN("[UavManager]: service call for setting control callbacks returned: %s.", srv.response.message.c_str());
+    2360             :     }
+    2361             : 
+    2362             :   } else {
+    2363           0 :     ROS_ERROR("[UavManager]: service call for setting control callbacks failed!");
+    2364             :   }
+    2365          94 : }
+    2366             : 
+    2367             : //}
+    2368             : 
+    2369             : /* ungripSrv() //{ */
+    2370             : 
+    2371          23 : void UavManager::ungripSrv(void) {
+    2372             : 
+    2373          23 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: ungripping payload");
+    2374             : 
+    2375          46 :   std_srvs::Trigger srv;
+    2376             : 
+    2377          23 :   bool res = sch_ungrip_.call(srv);
+    2378             : 
+    2379          23 :   if (res) {
+    2380             : 
+    2381           0 :     if (!srv.response.success) {
+    2382           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload returned: %s.", srv.response.message.c_str());
+    2383             :     }
+    2384             : 
+    2385             :   } else {
+    2386          23 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload failed!");
+    2387             :   }
+    2388          23 : }
+    2389             : 
+    2390             : //}
+    2391             : 
+    2392             : /* toggleControlOutput() //{ */
+    2393             : 
+    2394          71 : bool UavManager::toggleControlOutput(const bool& input) {
+    2395             : 
+    2396          71 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: toggling control output %s", input ? "ON" : "OFF");
+    2397             : 
+    2398         142 :   std_srvs::SetBool srv;
+    2399             : 
+    2400          71 :   srv.request.data = input;
+    2401             : 
+    2402          71 :   bool res = sch_toggle_control_output_.call(srv);
+    2403             : 
+    2404          71 :   if (res) {
+    2405             : 
+    2406          70 :     if (!srv.response.success) {
+    2407           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output returned: %s.", srv.response.message.c_str());
+    2408           0 :       return false;
+    2409             :     } else {
+    2410          70 :       return true;
+    2411             :     }
+    2412             : 
+    2413             :   } else {
+    2414           1 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output failed!");
+    2415           1 :     return false;
+    2416             :   }
+    2417             : }
+    2418             : 
+    2419             : //}
+    2420             : 
+    2421             : /* offboardSrv() //{ */
+    2422             : 
+    2423          67 : bool UavManager::offboardSrv(const bool in) {
+    2424             : 
+    2425          67 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: setting offboard to %d", in);
+    2426             : 
+    2427         134 :   std_srvs::Trigger srv;
+    2428             : 
+    2429          67 :   bool res = sch_offboard_.call(srv);
+    2430             : 
+    2431          67 :   if (!res) {
+    2432             : 
+    2433           1 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed!");
+    2434           1 :     return false;
+    2435             : 
+    2436             :   } else {
+    2437             : 
+    2438          66 :     if (!srv.response.success) {
+    2439           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed, returned: %s", srv.response.message.c_str());
+    2440           0 :       return false;
+    2441             :     } else {
+    2442          66 :       return true;
+    2443             :     }
+    2444             :   }
+    2445             : }
+    2446             : 
+    2447             : //}
+    2448             : 
+    2449             : /* disarmSrv() //{ */
+    2450             : 
+    2451           7 : void UavManager::disarmSrv(void) {
+    2452             : 
+    2453          14 :   std_srvs::SetBool srv;
+    2454             : 
+    2455           7 :   srv.request.data = false;
+    2456             : 
+    2457           7 :   bool res = sch_arm_.call(srv);
+    2458             : 
+    2459           7 :   if (res) {
+    2460             : 
+    2461           7 :     if (!srv.response.success) {
+    2462           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call disarming returned: %s.", srv.response.message.c_str());
+    2463             :     }
+    2464             : 
+    2465             :   } else {
+    2466           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for disarming failed!");
+    2467             :   }
+    2468           7 : }
+    2469             : 
+    2470             : //}
+    2471             : 
+    2472             : /* switchControllerSrv() //{ */
+    2473             : 
+    2474         183 : bool UavManager::switchControllerSrv(const std::string& controller) {
+    2475             : 
+    2476         183 :   ROS_INFO_STREAM("[UavManager]: activating controller '" << controller << "'");
+    2477             : 
+    2478         366 :   mrs_msgs::String srv;
+    2479         183 :   srv.request.value = controller;
+    2480             : 
+    2481         183 :   bool res = sch_switch_controller_.call(srv);
+    2482             : 
+    2483         183 :   if (res) {
+    2484             : 
+    2485         183 :     if (!srv.response.success) {
+    2486           2 :       ROS_WARN("[UavManager]: service call for switching controller returned: '%s'", srv.response.message.c_str());
+    2487             :     }
+    2488             : 
+    2489         183 :     return srv.response.success;
+    2490             : 
+    2491             :   } else {
+    2492             : 
+    2493           0 :     ROS_ERROR("[UavManager]: service call for switching controller failed!");
+    2494             : 
+    2495           0 :     return false;
+    2496             :   }
+    2497             : }
+    2498             : 
+    2499             : //}
+    2500             : 
+    2501             : /* switchTrackerSrv() //{ */
+    2502             : 
+    2503         184 : bool UavManager::switchTrackerSrv(const std::string& tracker) {
+    2504             : 
+    2505         184 :   ROS_INFO_STREAM("[UavManager]: activating tracker '" << tracker << "'");
+    2506             : 
+    2507             : 
+    2508         368 :   mrs_msgs::String srv;
+    2509         184 :   srv.request.value = tracker;
+    2510             : 
+    2511         184 :   bool res = sch_switch_tracker_.call(srv);
+    2512             : 
+    2513         184 :   if (res) {
+    2514             : 
+    2515         184 :     if (!srv.response.success) {
+    2516           2 :       ROS_WARN("[UavManager]: service call for switching tracker returned: '%s'", srv.response.message.c_str());
+    2517             :     }
+    2518             : 
+    2519         184 :     return srv.response.success;
+    2520             : 
+    2521             :   } else {
+    2522             : 
+    2523           0 :     ROS_ERROR("[UavManager]: service call for switching tracker failed!");
+    2524             : 
+    2525           0 :     return false;
+    2526             :   }
+    2527             : }
+    2528             : 
+    2529             : //}
+    2530             : 
+    2531             : /* landSrv() //{ */
+    2532             : 
+    2533           5 : bool UavManager::landSrv(void) {
+    2534             : 
+    2535           5 :   ROS_INFO("[UavManager]: calling for landing");
+    2536             : 
+    2537          10 :   std_srvs::Trigger srv;
+    2538             : 
+    2539           5 :   bool res = sch_land_.call(srv);
+    2540             : 
+    2541           5 :   if (res) {
+    2542             : 
+    2543           5 :     if (!srv.response.success) {
+    2544           0 :       ROS_WARN("[UavManager]: service call for landing returned: '%s'", srv.response.message.c_str());
+    2545             :     }
+    2546             : 
+    2547           5 :     return srv.response.success;
+    2548             : 
+    2549             :   } else {
+    2550             : 
+    2551           0 :     ROS_ERROR("[UavManager]: service call for landing failed!");
+    2552             : 
+    2553           0 :     return false;
+    2554             :   }
+    2555             : }
+    2556             : 
+    2557             : //}
+    2558             : 
+    2559             : /* elandSrv() //{ */
+    2560             : 
+    2561           1 : bool UavManager::elandSrv(void) {
+    2562             : 
+    2563           1 :   ROS_INFO("[UavManager]: calling for eland");
+    2564             : 
+    2565           2 :   std_srvs::Trigger srv;
+    2566             : 
+    2567           1 :   bool res = sch_eland_.call(srv);
+    2568             : 
+    2569           1 :   if (res) {
+    2570             : 
+    2571           1 :     if (!srv.response.success) {
+    2572           0 :       ROS_WARN("[UavManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    2573             :     }
+    2574             : 
+    2575           1 :     return srv.response.success;
+    2576             : 
+    2577             :   } else {
+    2578             : 
+    2579           0 :     ROS_ERROR("[UavManager]: service call for eland failed!");
+    2580             : 
+    2581           0 :     return false;
+    2582             :   }
+    2583             : }
+    2584             : 
+    2585             : //}
+    2586             : 
+    2587             : /* ehoverSrv() //{ */
+    2588             : 
+    2589           2 : bool UavManager::ehoverSrv(void) {
+    2590             : 
+    2591           2 :   ROS_INFO("[UavManager]: calling for ehover");
+    2592             : 
+    2593           4 :   std_srvs::Trigger srv;
+    2594             : 
+    2595           2 :   bool res = sch_ehover_.call(srv);
+    2596             : 
+    2597           2 :   if (res) {
+    2598             : 
+    2599           2 :     if (!srv.response.success) {
+    2600           0 :       ROS_WARN("[UavManager]: service call for ehover returned: '%s'", srv.response.message.c_str());
+    2601             :     }
+    2602             : 
+    2603           2 :     return srv.response.success;
+    2604             : 
+    2605             :   } else {
+    2606             : 
+    2607           0 :     ROS_ERROR("[UavManager]: service call for ehover failed!");
+    2608             : 
+    2609           0 :     return false;
+    2610             :   }
+    2611             : }
+    2612             : 
+    2613             : //}
+    2614             : 
+    2615             : /* takeoffSrv() //{ */
+    2616             : 
+    2617          20 : bool UavManager::takeoffSrv(void) {
+    2618             : 
+    2619          20 :   ROS_INFO("[UavManager]: calling for takeoff to height '%.2f m'", _takeoff_height_);
+    2620             : 
+    2621          40 :   mrs_msgs::Vec1 srv;
+    2622             : 
+    2623          20 :   srv.request.goal = _takeoff_height_;
+    2624             : 
+    2625          20 :   bool res = sch_takeoff_.call(srv);
+    2626             : 
+    2627          20 :   if (res) {
+    2628             : 
+    2629          20 :     if (!srv.response.success) {
+    2630           0 :       ROS_WARN("[UavManager]: service call for takeoff returned: '%s'", srv.response.message.c_str());
+    2631             :     }
+    2632             : 
+    2633          20 :     return srv.response.success;
+    2634             : 
+    2635             :   } else {
+    2636             : 
+    2637           0 :     ROS_ERROR("[UavManager]: service call for takeoff failed!");
+    2638             : 
+    2639           0 :     return false;
+    2640             :   }
+    2641             : }
+    2642             : 
+    2643             : //}
+    2644             : 
+    2645             : /* emergencyReferenceSrv() //{ */
+    2646             : 
+    2647          94 : bool UavManager::emergencyReferenceSrv(const mrs_msgs::ReferenceStamped& goal) {
+    2648             : 
+    2649          94 :   ROS_INFO_THROTTLE(1.0, "[UavManager]: calling for emergency reference");
+    2650             : 
+    2651         188 :   mrs_msgs::ReferenceStampedSrv srv;
+    2652             : 
+    2653          94 :   srv.request.header    = goal.header;
+    2654          94 :   srv.request.reference = goal.reference;
+    2655             : 
+    2656          94 :   bool res = sch_emergency_reference_.call(srv);
+    2657             : 
+    2658          94 :   if (res) {
+    2659             : 
+    2660          94 :     if (!srv.response.success) {
+    2661           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call for emergency reference returned: '%s'", srv.response.message.c_str());
+    2662             :     }
+    2663             : 
+    2664          94 :     return srv.response.success;
+    2665             : 
+    2666             :   } else {
+    2667             : 
+    2668           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for emergency reference failed!");
+    2669             : 
+    2670           0 :     return false;
+    2671             :   }
+    2672             : }
+    2673             : 
+    2674             : //}
+    2675             : 
+    2676             : }  // namespace uav_manager
+    2677             : 
+    2678             : }  // namespace mrs_uav_managers
+    2679             : 
+    2680             : #include <pluginlib/class_list_macros.h>
+    2681          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::uav_manager::UavManager, nodelet::Nodelet)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html b/mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..ce3c1f472f --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html @@ -0,0 +1,691 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.png b/mrs_uav_managers/src/uav_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e0bea592e1f2be2d4d108e4f406b750df0356ba1 GIT binary patch literal 7292 zcmV-?9E0PDP)Ss~uQULn~a-G7y9%ufOdBrIRz)Vz0hO{)qdgwPMTT!-uRI{yB5 zv1-5gR;1S!$^<9?R(AbrqdA~O*W!jTTHwtv3IW9!iV<_%nh`^0ZQQ1HHC+?m1V&h7S2@P(HZM%Yp3(QE?uEgKcei0o z^6RM9W8|C2T%^Y6RlusF#}&=swt(vh*RcQT2)++Ua{mU5LlCd@B}a_J-dn$)848eW z9S{pz_E`Q4WL4LN_j-2j1=IA!G^8{=havHcgf+gN-6`CW28_roDl<-Sb4w}8@Lwv z*tUDfAeY}Lz&L2YvE%}Wam=?k4lo-*57kXTdW8O`!!>ryjBP}Kl~pJPyaK?nGDZYg zF}X6pQ3_ZcqiZ^1Y7AF&ZBGmWRBf?A%*KR<0hAgalp-ZpZ?}82La5033utkVZp15d*dY;(9=N*NyyD#~8&JTLYD} z7&dDH0!n~nROL|&ScmKaOu|m^o;KnLlwv?}yMpbm4p?hl9>4Q!n;INMK0t>9tx~AH zj5W*WVO3a!{w`kNR zsk>JQ(TQ1g#jIT$hvMSM6Af(GUWjq5EvB{k{A`zV?4%dv48MxRA+l0&v`xMR((KmYlQYgsQo=t`T5maa}SKfzi$YWB3+bcY^23BzF$j zQ{A)6l~5xF!Es#|vBlcVAi$a;y$Vng%bImb^DzF#=6Edu3>38bE&Xc)FHV*^AcP{0 z0sS_sa077Yu3-3PDX%0+#y;hDrKn*uXN12;gqlb|S{_9nBm=0Rvcy>Mkyhaw`e1tA zrQp=|fXK2;OK9_aW838xpm-PuSV59ulmhO=$5%6}-i3U0ZQ4ln?RP#Fs?9l|93yNs zz&?zNV*(2p2R^gCXJR&!6qj8?PO*Tq$H)k1*>w}pW8@?rjWH5K%w!+K8Sz+K?-Wjj z6i18)TGxge60d7sLv~JKCJ37`PT3bT2Gn6p^3{-VAc*xflK}aybz{VhQUfF)gj{NX zQx2sZWBUDR=Edh1BQ?m9otK+%8OkLbj7?5KZN4p2GtWK6YW)40Tf}Z?bj-*2On|q1<(WF!59Ky z>`KU{lLiD9cbz>Xpp+Pbk5hqnvByYKUDy>XrzC(YZW-V#1e_#YhYwxK{uk9WL%dL9 zUGDcE&>p}4z5f1rzW)FH^z`3&0Kj{-UGC31Km^>N%{u2gut6|0lN8Va+st*^Sn3jv zfQ4gJ5o53lcOBzCfL2roL2Cm}yNywCwy_e=?`nkIL-1$TDEs8)^6vWWKUq?nr%(Vm zsph(YF}clhwfBF!2Bu02KTj? zI~5Fz5Feo?paA83B#b*M2$)+^t1GI#&*}`!lZsoTTX(%APHr_qsI!qTee{AOJui#=bF{)GX4scy*YeGzr>$=F&VZVTH)fDIy3xUi1L{|yewzn>q%o`?Y zp5YkNte~_jMGOUq>Y5eo0UR%S?81n0#;KSpBmTn5!vClleyK!0G?MD1za^tF`#-WBu^Y-qX2s_Ru3ZH>Yh66+RFh$ZUPiKXD^|6{xI$N^MFEueiGNt z4=DEiZ62^KXTN~5MWa|Vlzr{wBhG|NEZ&M}YN(Y!oYjR$Tn|%@r2v#;baBc0RB99e zGlyQ?`9H5BZQ0alW+qn^-2g8|d4{m3p;n zxH{h*u_m1T@lq~@iICdRbv7*0^dc6Q2b^$u6Aefa^FIJ{mHLuLeOR$r{1D;Vlo$mX zK!=geBH{hB1PuD~#g)0IdUjn4%_Ntm-Jv>r@*9o^d_19n=Qpg!1MGbmV?gYVt|0U) zphQ8+i76+Blp;cKg{$!OGx7}GZei}Mk(RwUge>lwvq-Zwq!Cp(=HVqpxza(e$3-FzKW9VQ!i-9#VRq7>cI6S>Wpdq)HBBHxFWe`klxCN>9m?w zKW1J%Ew~ztwfb}(FxFra7$pH;&2r`EaE|t~Ce(DXR`w-#=7oy9UWD9HvL_QWxGLj! zw{OEVU?)bB3EfMfcFbRckxIyAY4kpfZNOdE49D^QDVzg``j60O7^{G`Yvy9T{=M(| za28o=jI}4R&AhSqivB$^qi-9=&>q0eeuVElqoFCm_-dlPaP9ixnDeZdraEHoKbSKV zW+lelzW#yiX8~QmFTfI`B%-MjGky-A?(3&^3U_pTe!_}i7;fvPtUV){c&3kFVQR&! zY&2jeMv@8LOMzWq8zd&>-amyQGMVG;$5;iFt{L-myz!oy#q6Oj ziyU3IBt=1(HEWiyN zjhHGYXoV?px!~To2*3;+5uZ(2By#r7^3X6bt(@w|%yr0E1=fS&)c*7pz}`)D<;Y22 zfnR}<2kgOE1+|#92XiWGe{(5twOTQC z-NT-lRK8Qd_)e;C^=gkuj9VO825w7bL5vCw;WA5ld1HM`2hHOc7wSAb16a&*d6}h@ z7_O`_??J;J@tdgPxz?671_rT4oV2Wwb>iIT$4@HT;bI-Rle@>=iW8VK=F(QxWo--4 za%2%l1QgZI!GhD`QmhP6swsrEbN7p|cJBWEg3~GSoadvEJ$!|hH+AQE(w1nUdE+Xy z3cy?xSwN%fG z?j2SH>g<7QrBvbBOnb6`)`3cK%PziWRb~I0*-PVwBKa;m9E&5nGs-v3(``oD;?Qn$ zAYcN89NSKQzS|LJ{qWNfKOJ!zaPwTB9+DA8aI2fmd!xk3$7EbD zeYd&^J*2O^IfOj3142|<=WKmP58@&Kbr_f0hL8hNweUS%`{P{kvGoDeSC24>COHVC z>pB*Z!h&<6npD3lJRWuF&9{%nVM$HbVT*IeD^sz1e-N;>5Z}ASacc?(e-C#1lU10D zI=vpgfMKqKM7ij|P_9_t9QlB4@`tpGP~1TxaWL?KS_UY`7)O+y$K4RK&xs_RrEU)i za%oz{mhDhZ*JtSs~OE@naBp8h`P2Q5Xb&Qq+d^KyL3x9@l81{Y>dF`84 zXq#QBd~xs3Qe6DVWuL~6+03ph%u|T-#9aZ3pc;j8$P@x^_AXm zJvO5Ply^-GeY$xg%`L}CN6ZbXhk(5-+00Z5XH<14DPrfvx2+_MZ9pl;y{+aXY$erF z_N;i=+(-`C3TO0WAnoEk?xxrwJ#fZ7?3=G<3u2JZRW24i#&{a2ek!Ol0a1A_qSpoR z^H~L~JyN0Xnz>XK`XM@ul3M8%Vr0s1L`V_xf2kh!?3F!i~_)}F><)o7zN=6dmS!sJz~bc z0nsgHHWh5AnAHK_C1x_3)f)Fbd9FKo%@q9BL%C@dZ$N;fl;N1=a|1@39D7cw2}aZE z*@J{&t2Q8S!x4UqHC=;Z2hoEy`*pjnZngj9Dmsify2s$lDoo^TP@ge#T(7{$^P z_&pf$TW-06aIe+#;5bmhyUln10z|U$sQVth!vU&v4YueRCC$zq(%^APsQKBSTj2+K0f*Ovi;g zFD`AeM|YS&q>W^cfnOhRd_~V>JpRTEhb@quG7EnsedhC4ggse|^aWuK&>-ER+ zczSqb6cu8}ETKEmUZGl2S=3jqTn*$pZ`jF~ui!>}TtA14OEcDnIyP@T&so>Z^N-~i z-OSm%_mQ1mYXS-|YPzORduD)ddvxRCUl|XpmG7Wyxz0?|m0GSepso^-UarI&w4!>d z|G&7V*?<3BT%xWIu(;g+etiw`%91fujEo*|goPtR$kC>a#0jp5-P-*c?^aPCl*A1F z7M9U(U-SkRMxU;139#&uq&6RciprS+nnp>b?6AcR&`$S^o_S-;wGaMF*fsRnEv67N zXb2VtMYz=a0BSkc$bC1(-g0&3f0A9BKvsRaae#AV{}nL#hX7Do9x9ec*=h1UHze*j zWsY&y0JfA=#_BjTs8~Wzb?t^hqH8H&1+8|uCM6~m3hVtEI7eGvwAu3##%i=6MuuIu zAeH`-fRbo|HsC7@A&Z(5iZQBhZ!)Ckqpvx8KK;6_wq%XEV*Pl;k{G(OfZ+H8P@kyYC*#xP_yJ2|h;o=8 za3V)T^_QgBJ1Pl=58zq?N}(w0SOv%Lq`@ z^%ok$JDwWy%NBn+rZLPM{5Xwam-*nX+qwhQ{7i-Rh5r3A)hunNQ&9!9UkkVGPDOdp z=5#7fry}R$M>-WxpfvudrZ}C7DqtTUYst@h&d0S}x8&8y$0eDV@^#%{&*Axa3@0~t z3+ScLgouDxL-|Fv1!IS=P%9VE)>D8&kCD2%WyD>>{z(#?DsyBML=*XGY0X^&|=e0-Kw`Qo+`=?0QNY! zKk1)Ti1hT^jl$DZ`mU{I{nook-k2Jaoh0SMI4HVF17cwZ^7y}_u92LhMO}TGg?-a5 zo0!OgGwOOqT~Y65#C%oMHEc+t?cxx$Rz5*Y2)q0xtPVW0-0B(XL?<+VyQhBkHk5#EG`RK20WvM>-*p88gf5KfEVB~+4 zk9%iR_~Yc?mIV+CZr}R~YakUAxjjQp<=go98QGLdKIM;Uiq7{HJa2*xKxL)=G#^=3 z(tIRXdGc|oUiY8#@yVS0_%c2oBP&H;ZqR*U(s{XoP0NBV2KO7(f-dkU#|pYMLgyU^ zmg<2IUG5`XLKg2y21EACtuvcaO{bKH23WvZkf$s$*6x4YBP)f4m3qDO)~lrIQLYGB zr|QotDXgQeSFE(U-tzU;2UcUmDh_A)q2#N>v)}ouM1}9J1jK=bwW4I_s&yn$gdBdU*Az#q_UkWV6@u{?_qld17uD>ywP<1;-?+nwPs8n zi@4jYnMZte@LV%i4Zk!{0ql8cV4^O@RZGuJSm2{sT*3;Lo3Mf<0m@6Fw!PIkOC))wyy%*@8 z+f)}tHG)IMul2%+nx{Zns;at;QCPlxPXy!fFfctj7}C!F>*#O zy%Y!|x4UZ_=S*Zk>NKvc0eQd63`krjRk?$Os*@s51ME?(w8y(rdFtHY{c8~1V^`uq zQbRB?WRcE6jG+uozf~v=%@CSNu?qS@?eOCe_Kh#2K{%YpvB;7}&9i>wwpSnK2GUal zlz081`I&&?v71Z!@sZ*2nuIpw5Qx?h8P0s4-nq7q4Bsdd*9xQNq3k)V@uGN%7-Qlp z47m4(n#kwu0^mE}mZlUb(T|G@!xu;0uMQza=Dxy;w8|c(B4rZhsT*CeAX;!d+Fp%o zo!v`~S9?IYR`jDx?cFVv{o_>j;YHRf2x9mT>oUsgqzt6TcP)4?J>-MN_|Eu9s{s9m z1eVq&V!+}uyNV!J_F_Pu7z9%}Yp)-pW{X=wUWE*OW|aJqEiT&s1K7bz!Nsh}?*Tzn z3yF!n4Z-~g_wuBF2?bzrXMsp|ZlTuIZ`kn!w3j6kW565ul*K^$+-5!#cNn)h`pxO( z?Nn9ST~TS7^WmS2o{*c^A!Z9StX+7;Uu619m8eS76S_K}8e{Da6fMSyaMZUXn^smy z0L2)yw_R93Y@7Kzsa!%iT2`;W_BcT+4SZtr(H$>z(aLb$p!-XHA&6L|h#9_bB+e!j zM=OBY-+IwQHK_pn7n46f@meZ>LIG^T_<;%_9pwF71@QcS1z>lXOrZeSlb>R$3k9%1 zGqTq#G*Mrt3ktQqd5q0%*jZ|dr6BFv{)SUS z{pda%LqC3D4f`?tnFE^_<__Y?wem3e)tdmOx&K-M^4C$ zruQ&IR?}O1T8vXOa~3p3rHE8Z%=d&9Z-w=d3iHz}t)8Z}uYj}C7W#3|3)>l8;!9dI zB{3pvMsaKUfoux1Vr5cE%}Kc=1KmJL%^~?dDG%C+Tdy{&04k_1wHYypuh3@Ui&L9X z+ALnWd1|u;#=+P@%C%Wl!e!D^E(Duf-Ig7rGZwblhj>Uuf;N_F>~mA>fprhQHu=;& z4U9Y7;D>Y%|AL5EdKleffpNdcumQ#o)ICV`oVq8|J=u-zr|xNBJW^yB>z++jpS$ax z53F>ts!R>#pQrB0?H+s@>eM|AjDuZ3r7%_WKcyR?T)as_EZnXc&B@_>v-VR z;9Yy7T5$`;4^ex0dOs!jPqkO8EO?%3uii#~*`zb&%rR2uh1Ct72d!cy)NQWb*7gqp WUi=zIz>!!00000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::garmin_agl::GarminAgl::GarminAgl()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().273
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html new file mode 100644 index 0000000000..0cbe65b17f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::garmin_agl::GarminAgl::GarminAgl()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().273
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.frameset.html new file mode 100644 index 0000000000..4e43b37ae4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html new file mode 100644 index 0000000000..be71d48c8e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html @@ -0,0 +1,159 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_STATE_GARMIN_AGL_H
+       2             : #define ESTIMATORS_STATE_GARMIN_AGL_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <mrs_msgs/Float64Stamped.h>
+       9             : 
+      10             : #include <mrs_lib/lkf.h>
+      11             : #include <mrs_lib/profiler.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : #include <mrs_lib/subscribe_handler.h>
+      14             : #include <mrs_lib/publisher_handler.h>
+      15             : #include <mrs_lib/attitude_converter.h>
+      16             : #include <mrs_lib/transformer.h>
+      17             : 
+      18             : #include <mrs_uav_managers/agl_estimator.h>
+      19             : 
+      20             : #include <mrs_uav_state_estimators/estimators/altitude/alt_generic.h>
+      21             : 
+      22             : //}
+      23             : 
+      24             : namespace mrs_uav_state_estimators
+      25             : {
+      26             : 
+      27             : namespace garmin_agl
+      28             : {
+      29             : const char name[]         = "garmin_agl";
+      30             : const char frame_id[]     = "garmin_agl_origin";
+      31             : const char package_name[] = "mrs_uav_state_estimators";
+      32             : 
+      33             : const bool is_core_plugin = true;
+      34             : 
+      35             : class GarminAgl : public mrs_uav_managers::AglEstimator {
+      36             : 
+      37             : private:
+      38             :   std::unique_ptr<AltGeneric> est_agl_garmin_;
+      39             :   const std::string           est_agl_name_ = "garmin_agl";
+      40             : 
+      41             :   const bool is_core_plugin_;
+      42             : 
+      43             :   ros::Timer timer_update_;
+      44             :   int        _update_timer_rate_;
+      45             :   void       timerUpdate(const ros::TimerEvent &event);
+      46             : 
+      47             :   ros::Timer timer_check_health_;
+      48             :   int        _check_health_timer_rate_;
+      49             :   void       timerCheckHealth(const ros::TimerEvent &event);
+      50             : 
+      51             :   bool isConverged();
+      52             : 
+      53             :   void waitForEstimationInitialization();
+      54             : 
+      55             : public:
+      56          73 :   GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name), is_core_plugin_(is_core_plugin) {
+      57          73 :   }
+      58             : 
+      59         146 :   ~GarminAgl(void) {
+      60         146 :   }
+      61             : 
+      62             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+      63             :   bool start(void) override;
+      64             :   bool pause(void) override;
+      65             :   bool reset(void) override;
+      66             : 
+      67             :   mrs_msgs::Float64Stamped getUavAglHeight() const override;
+      68             :   std::vector<double>      getHeightCovariance() const override;
+      69             : };
+      70             : 
+      71             : }  // namespace garmin_agl
+      72             : 
+      73             : }  // namespace mrs_uav_state_estimators
+      74             : 
+      75             : #endif  // ESTIMATORS_STATE_GARMIN_AGL_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html new file mode 100644 index 0000000000..01180ec6c9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html @@ -0,0 +1,39 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1d069e474cef144181fc5cb4a7752617234f2348 GIT binary patch literal 380 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1{!VDxu^<<9$DTx4|5ZC|z|E~gq{) z-+wzRyyepwr_aj9N0zi)yIA@DruK$xCbdH!RA#*~FEu*Kt+Mr|wp)bOzeHP(-D@^_ zujRkIGNJuOnSSTr%L_6h^0%b~7kbxKyUa3OW%x$$>|Ep5oPyiLY^#!;A~i6^vZ+X|Ua&y>FQExYwM!Ofs>mTmP>_Uf+f44=fmvnFp3 VG2vV8@g5jd44$rjF6*2Ung9 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
<unnamed>100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html new file mode 100644 index 0000000000..516cbbd304 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
<unnamed>100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html new file mode 100644 index 0000000000..602f0691e0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
<unnamed>100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html new file mode 100644 index 0000000000..e87678457a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html new file mode 100644 index 0000000000..c2ca95a2ae --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html new file mode 100644 index 0000000000..dcf4c3fdd0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html new file mode 100644 index 0000000000..b15664380f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::AltGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)172
mrs_uav_state_estimators::AltGeneric::~AltGeneric()172
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2172
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html new file mode 100644 index 0000000000..7c92a0e865 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::AltGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)172
mrs_uav_state_estimators::AltGeneric::~AltGeneric()172
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2172
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.frameset.html new file mode 100644 index 0000000000..ffdd0b59a3 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html new file mode 100644 index 0000000000..8d25165a8a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html @@ -0,0 +1,245 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_ALTITUDE_ALT_GENERIC_H
+       2             : #define ESTIMATORS_ALTITUDE_ALT_GENERIC_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <nav_msgs/Odometry.h>
+       9             : #include <sensor_msgs/Range.h>
+      10             : 
+      11             : #include <mrs_lib/lkf.h>
+      12             : #include <mrs_lib/repredictor.h>
+      13             : #include <mrs_lib/profiler.h>
+      14             : #include <mrs_lib/param_loader.h>
+      15             : #include <mrs_lib/subscribe_handler.h>
+      16             : 
+      17             : #include <functional>
+      18             : 
+      19             : #include <mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h>
+      20             : #include <mrs_uav_state_estimators/estimators/correction.h>
+      21             : 
+      22             : #include <mrs_uav_state_estimators/AltitudeEstimatorConfig.h>
+      23             : 
+      24             : //}
+      25             : 
+      26             : namespace mrs_uav_state_estimators
+      27             : {
+      28             : 
+      29             : namespace alt_generic
+      30             : {
+      31             : 
+      32             : const int n_states       = 3;
+      33             : const int n_inputs       = 1;
+      34             : const int n_measurements = 1;
+      35             : 
+      36             : }  // namespace alt_generic
+      37             : 
+      38             : class AltGeneric : public AltitudeEstimator<alt_generic::n_states> {
+      39             : 
+      40             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      41             : 
+      42             :   typedef mrs_lib::DynamicReconfigureMgr<AltitudeEstimatorConfig> drmgr_t;
+      43             : 
+      44             :   using lkf_t         = mrs_lib::LKF<alt_generic::n_states, alt_generic::n_inputs, alt_generic::n_measurements>;
+      45             :   using varstep_lkf_t = mrs_lib::varstepLKF<alt_generic::n_states, alt_generic::n_inputs, alt_generic::n_measurements>;
+      46             :   using A_t           = lkf_t::A_t;
+      47             :   using B_t           = lkf_t::B_t;
+      48             :   using H_t           = lkf_t::H_t;
+      49             :   using Q_t           = lkf_t::Q_t;
+      50             :   using x_t           = lkf_t::x_t;
+      51             :   using P_t           = lkf_t::P_t;
+      52             :   using u_t           = lkf_t::u_t;
+      53             :   using z_t           = lkf_t::z_t;
+      54             :   using R_t           = lkf_t::R_t;
+      55             :   using statecov_t    = lkf_t::statecov_t;
+      56             : 
+      57             :   typedef mrs_lib::Repredictor<varstep_lkf_t> rep_lkf_t;
+      58             : 
+      59             :   using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;
+      60             : 
+      61             : private:
+      62             :   std::string parent_state_est_name_;
+      63             : 
+      64             :   double                                      dt_;
+      65             :   double                                      input_coeff_, default_input_coeff_;
+      66             :   A_t                                         A_;
+      67             :   B_t                                         B_;
+      68             :   H_t                                         H_;
+      69             :   Q_t                                         Q_;
+      70             :   std::shared_ptr<lkf_t>                      lkf_;
+      71             :   std::unique_ptr<rep_lkf_t>                  lkf_rep_;
+      72             :   std::vector<std::shared_ptr<varstep_lkf_t>> models_;
+      73             :   mutable std::mutex                          mutex_lkf_;
+      74             :   statecov_t                                  sc_;
+      75             :   mutable std::mutex                          mutex_sc_;
+      76             : 
+      77             :   std::unique_ptr<drmgr_t> drmgr_;
+      78             :   void                     callbackReconfigure(AltitudeEstimatorConfig &config, [[maybe_unused]] uint32_t level);
+      79             : 
+      80             :   z_t                innovation_;
+      81             :   mutable std::mutex mtx_innovation_;
+      82             : 
+      83             :   bool          is_error_state_first_time_ = true;
+      84             :   ros::Duration error_state_duration_;
+      85             :   ros::Time     prev_time_in_error_state_;
+      86             : 
+      87             :   bool is_repredictor_enabled_;
+      88             :   int  rep_buffer_size_ = 200;
+      89             : 
+      90             :   const bool is_core_plugin_;
+      91             : 
+      92             :   std::vector<std::string>                                              correction_names_;
+      93             :   std::vector<std::shared_ptr<Correction<alt_generic::n_measurements>>> corrections_;
+      94             : 
+      95             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput> sh_control_input_;
+      96             :   void                                                timeoutCallback(const std::string &topic, const ros::Time &last_msg);
+      97             :   std::atomic<bool>                                   is_input_ready_ = false;
+      98             : 
+      99             :   ros::Timer timer_update_;
+     100             :   void       timerUpdate(const ros::TimerEvent &event);
+     101             : 
+     102             :   ros::Timer timer_check_health_;
+     103             :   void       timerCheckHealth(const ros::TimerEvent &event);
+     104             : 
+     105             :   void doCorrection(const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id);
+     106             :   void doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp);
+     107             : 
+     108             :   bool isConverged();
+     109             : 
+     110             :   Q_t                getQ();
+     111             :   mutable std::mutex mtx_Q_;
+     112             : 
+     113             : public:
+     114         172 :   AltGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+     115         172 :       : AltitudeEstimator<alt_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+     116         172 :   }
+     117             : 
+     118         344 :   ~AltGeneric(void) {
+     119         344 :   }
+     120             : 
+     121             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     122             :   bool start(void) override;
+     123             :   bool pause(void) override;
+     124             :   bool reset(void) override;
+     125             : 
+     126             :   double getState(const int &state_idx_in) const override;
+     127             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     128             : 
+     129             :   void setState(const double &state_in, const int &state_idx_in) override;
+     130             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     131             : 
+     132             :   states_t getStates(void) const override;
+     133             :   void     setStates(const states_t &states_in) override;
+     134             : 
+     135             :   double getCovariance(const int &state_idx_in) const override;
+     136             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     137             : 
+     138             :   covariance_t getCovarianceMatrix(void) const override;
+     139             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     140             : 
+     141             :   double getInnovation(const int &state_idx) const override;
+     142             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     143             : 
+     144             :   void setDt(const double &dt);
+     145             :   void setInputCoeff(const double &input_coeff);
+     146             : 
+     147             :   void generateRepredictorModels(const double input_coeff);
+     148             : 
+     149             :   void generateA();
+     150             :   void generateB();
+     151             : 
+     152             :   void timeoutOdom(const std::string &topic, const ros::Time &last_msg, const int n_pubs);
+     153             :   void timeoutRange(const std::string &topic, const ros::Time &last_msg, const int n_pubs);
+     154             : 
+     155             :   std::string getNamespacedName() const;
+     156             : 
+     157             :   std::string getPrintName() const;
+     158             : };
+     159             : }  // namespace mrs_uav_state_estimators
+     160             : 
+     161             : #endif  // ESTIMATORS_ALTITUDE_ALT_GENERIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html new file mode 100644 index 0000000000..85adff98de --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html @@ -0,0 +1,61 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..dcf0f4a027a6ab756dec1329ddcee71c4cae2957 GIT binary patch literal 688 zcmV;h0#E&kP)bm(Ppe}^qLnGb+F$2vKD02Gn zh($1dgra5Z8E?E&cRG}^Kx#yMfS#cr8p4)R${TUN=r@|=k$axQF7Ej(BA)90}BGAm^jcLl0&P&><5?5@?l)W15 z*ceT@60t-DKw?Uo)d&Z34^8KOsIJS88X1*Zxdf%Fo*cRt+v{ zz{VnN=>k`O353Z4;lz0bk%xtVdKj)gkW5@kI5QgK1#1)T1NCV5ExZAu!6Gf-yUk(o zI_q696*4h1pK!os(Q5%Z&B8@AX`^Yse#EiF#jklyiXJZuc49O|P?=mpMt2K+B4&}! zDxRsQYO{&!#ej+FIWojMZCbMd#E!FR5=8x3=)}zT{5YwaS~4UJFz(3@Jo&slhq@gc zRlV#zM0B&hYSzt`XtsK>y-#`N|2_WR`Tsn3+qUg*EH8d=+rF8^*);g*{x-0$lK(o|Y&0b=W0s!=7T&CEV>Oy!9ePcyge|x|(9`l-aMLnFyWzjy1 z9kVSV@LD0}Sxbt6RK_&i8dzK_FLFMex4`W9Oz}Zykn(SMU9|PPztmjnaL`kYvE`jB zAM+>D)!$NuGqnlj6xLY@*6Y0R)+;pj!HIF@(PL*ABk5r>>k*&qo>xvX(Os97EA9_V W6RZS^kluj+0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator()0
mrs_uav_state_estimators::AltitudeEstimator<3>::AltitudeEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)172
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2172
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html new file mode 100644 index 0000000000..0f6f6f86bb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltitudeEstimator<3>::AltitudeEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)172
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator()0
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2172
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.frameset.html new file mode 100644 index 0000000000..c89e621d6c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html new file mode 100644 index 0000000000..7cafb526aa --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html @@ -0,0 +1,130 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef ESTIMATORS_ALTITUDE_ALTITUDE_ESTIMATOR_H
+       3             : #define ESTIMATORS_ALTITUDE_ALTITUDE_ESTIMATOR_H
+       4             : 
+       5             : /* includes //{ */
+       6             : 
+       7             : #include <mrs_uav_state_estimators/estimators/partial_estimator.h>
+       8             : 
+       9             : //}
+      10             : 
+      11             : namespace mrs_uav_state_estimators
+      12             : {
+      13             : 
+      14             : namespace altitude
+      15             : {
+      16             : const char type[] = "ALTITUDE";
+      17             : 
+      18             : typedef enum
+      19             : {
+      20             :   ODOMETRY,
+      21             :   RANGE
+      22             : } MeasurementType_t;
+      23             : 
+      24             : }  // namespace altitude
+      25             : 
+      26             : template <int n_states>
+      27             : class AltitudeEstimator : public PartialEstimator<n_states, 1> {
+      28             : 
+      29             : protected:
+      30         172 :   AltitudeEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(altitude::type, name, frame_id) {
+      31         172 :   }
+      32             : 
+      33         172 :   virtual ~AltitudeEstimator(void) {
+      34         172 :   }
+      35             : 
+      36             : 
+      37             : private:
+      38             :   static const int _n_axes_   = 1;
+      39             :   static const int _n_states_ = n_states;
+      40             :   static const int _n_inputs_;
+      41             :   static const int _n_measurements_;
+      42             : };
+      43             : 
+      44             : }  // namespace mrs_uav_state_estimators
+      45             : 
+      46             : #endif  // ESTIMATORS_ALTITUDE_ALTITUDE_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..90f903e2e9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.overview.html @@ -0,0 +1,32 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..073c144eb9b7fd83c98f25c4b8ee68b76b771829 GIT binary patch literal 295 zcmeAS@N?(olHy`uVBq!ia0vp^0YI$B!VDyR#hdkklth3}i0l9V|5pLQ^7pZ^ul_SI ztOAOIsdL_&mjc_7hJ zpPIm+6jaE2@rX$-yY!@v4IF1DcGzC)G&r(S?f--q#;aZk+!wv66Ek(YMkDcSrxo*i_1-pY!}fjJhOH86XRDi kH_kk+@!`k+9d9o(CNo-GFMMqo0`vfbr>mdKI;Vst08ROJ-T(jq literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html new file mode 100644 index 0000000000..fbc4d6c811 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
altitude_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
alt_generic.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
<unnamed>100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html new file mode 100644 index 0000000000..dcc7f63b51 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
altitude_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
alt_generic.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
<unnamed>100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html new file mode 100644 index 0000000000..43f8951adc --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
<unnamed>100.0 %5 / 5100.0 %3 / 3
altitude_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html new file mode 100644 index 0000000000..af4e7295a1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
altitude_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
alt_generic.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html new file mode 100644 index 0000000000..fe39820fab --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
altitude_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
alt_generic.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html new file mode 100644 index 0000000000..c725c1fa5c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
altitude_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html new file mode 100644 index 0000000000..f87866e792 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37372751.3 %
Date:2024-04-25 05:42:57Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getAvgRtkInitZ(double)0
mrs_uav_state_estimators::Correction<2>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)0
mrs_uav_state_estimators::Correction<2>::resetProcessors()0
mrs_uav_state_estimators::Correction<2>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getAvgRtkInitZ(double)22
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)99
mrs_uav_state_estimators::Correction<2>::Correction(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_uav_state_estimators::EstimatorType_t const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, std::function<double (int, int)>, std::function<void (mrs_uav_state_estimators::Correction<2>::MeasurementStamped, double, mrs_uav_managers::estimation_manager::StateId_t)>)103
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const206
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const210
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)233
mrs_uav_state_estimators::Correction<1>::Correction(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_uav_state_estimators::EstimatorType_t const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, std::function<double (int, int)>, std::function<void (mrs_uav_state_estimators::Correction<1>::MeasurementStamped, double, mrs_uav_managers::estimation_manager::StateId_t)>)251
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const441
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const502
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const568
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const990
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1770
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1803
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1803
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3746
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3826
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const3828
mrs_uav_state_estimators::Correction<2>::getRawCorrection()6605
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()6605
mrs_uav_state_estimators::Correction<1>::getRawCorrection()8345
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()8345
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10826
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10854
mrs_uav_state_estimators::Correction<2>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)10856
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)187672
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)191442
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)195649
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)195715
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)195772
mrs_uav_state_estimators::Correction<2>::isMsgComing()198062
mrs_uav_state_estimators::Correction<2>::isHealthy()198067
mrs_uav_state_estimators::Correction<2>::setR(double)198737
mrs_uav_state_estimators::Correction<2>::getStateId() const198967
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)202212
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)206111
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)266160
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)269677
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)404743
mrs_uav_state_estimators::Correction<1>::setR(double)459648
mrs_uav_state_estimators::Correction<1>::getStateId() const459648
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)463329
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)467282
mrs_uav_state_estimators::Correction<1>::isMsgComing()476352
mrs_uav_state_estimators::Correction<1>::isHealthy()476590
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)926875
mrs_uav_state_estimators::Correction<2>::getR()1007870
mrs_uav_state_estimators::Correction<1>::getR()1386154
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html new file mode 100644 index 0000000000..71aaa2faa9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37372751.3 %
Date:2024-04-25 05:42:57Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1770
mrs_uav_state_estimators::Correction<1>::isMsgComing()476352
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)266160
mrs_uav_state_estimators::Correction<1>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)195649
mrs_uav_state_estimators::Correction<1>::getAvgRtkInitZ(double)22
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)463329
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)195772
mrs_uav_state_estimators::Correction<1>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getRawCorrection()8345
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)926875
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1803
mrs_uav_state_estimators::Correction<1>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)269677
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()8345
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)233
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)195715
mrs_uav_state_estimators::Correction<1>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getR()1386154
mrs_uav_state_estimators::Correction<1>::setR(double)459648
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)467282
mrs_uav_state_estimators::Correction<1>::isHealthy()476590
mrs_uav_state_estimators::Correction<1>::Correction(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_uav_state_estimators::EstimatorType_t const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, std::function<double (int, int)>, std::function<void (mrs_uav_state_estimators::Correction<1>::MeasurementStamped, double, mrs_uav_managers::estimation_manager::StateId_t)>)251
mrs_uav_state_estimators::Correction<2>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3746
mrs_uav_state_estimators::Correction<2>::isMsgComing()198062
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)187672
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)10856
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10826
mrs_uav_state_estimators::Correction<2>::getAvgRtkInitZ(double)0
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)202212
mrs_uav_state_estimators::Correction<2>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)0
mrs_uav_state_estimators::Correction<2>::resetProcessors()0
mrs_uav_state_estimators::Correction<2>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getRawCorrection()6605
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)404743
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3826
mrs_uav_state_estimators::Correction<2>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)191442
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()6605
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)99
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10854
mrs_uav_state_estimators::Correction<2>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getR()1007870
mrs_uav_state_estimators::Correction<2>::setR(double)198737
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)206111
mrs_uav_state_estimators::Correction<2>::isHealthy()198067
mrs_uav_state_estimators::Correction<2>::Correction(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, mrs_uav_state_estimators::EstimatorType_t const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, std::function<double (int, int)>, std::function<void (mrs_uav_state_estimators::Correction<2>::MeasurementStamped, double, mrs_uav_managers::estimation_manager::StateId_t)>)103
mrs_uav_state_estimators::Correction<1>::getStateId() const459648
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const568
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const990
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1803
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const502
mrs_uav_state_estimators::Correction<2>::getStateId() const198967
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const210
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const441
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const3828
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const206
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.frameset.html new file mode 100644 index 0000000000..3b2b9aa9cc --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html new file mode 100644 index 0000000000..b88084eac5 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html @@ -0,0 +1,1948 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37372751.3 %
Date:2024-04-25 05:42:57Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef ESTIMATORS_CORRECTION_H
+       3             : #define ESTIMATORS_CORRECTION_H
+       4             : 
+       5             : #include <Eigen/Dense>
+       6             : 
+       7             : #include <mrs_lib/subscribe_handler.h>
+       8             : #include <mrs_lib/publisher_handler.h>
+       9             : #include <mrs_lib/attitude_converter.h>
+      10             : #include <mrs_lib/param_loader.h>
+      11             : #include <mrs_lib/dynamic_reconfigure_mgr.h>
+      12             : #include <mrs_lib/gps_conversions.h>
+      13             : #include <mrs_lib/mutex.h>
+      14             : #include <mrs_lib/geometry/cyclic.h>
+      15             : 
+      16             : #include <mrs_msgs/RtkGps.h>
+      17             : #include <mrs_msgs/EstimatorCorrection.h>
+      18             : #include <mrs_msgs/Float64Stamped.h>
+      19             : 
+      20             : #include <sensor_msgs/Range.h>
+      21             : #include <sensor_msgs/Imu.h>
+      22             : #include <nav_msgs/Odometry.h>
+      23             : 
+      24             : #include <std_srvs/SetBool.h>
+      25             : 
+      26             : #include <functional>
+      27             : 
+      28             : #include <mrs_uav_managers/estimation_manager/types.h>
+      29             : #include <mrs_uav_managers/estimation_manager/support.h>
+      30             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      31             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      32             : 
+      33             : #include <mrs_uav_state_estimators/processors/processor.h>
+      34             : #include <mrs_uav_state_estimators/processors/proc_median_filter.h>
+      35             : #include <mrs_uav_state_estimators/processors/proc_saturate.h>
+      36             : #include <mrs_uav_state_estimators/processors/proc_excessive_tilt.h>
+      37             : #include <mrs_uav_state_estimators/processors/proc_tf_to_world.h>
+      38             : 
+      39             : #include <mrs_uav_state_estimators/CorrectionConfig.h>
+      40             : 
+      41             : 
+      42             : namespace mrs_uav_state_estimators
+      43             : {
+      44             : 
+      45             : typedef enum
+      46             : {
+      47             :   LATERAL,
+      48             :   ALTITUDE,
+      49             :   HEADING
+      50             : } EstimatorType_t;
+      51             : const int n_EstimatorType_t = 3;
+      52             : 
+      53             : typedef enum
+      54             : {
+      55             :   UNKNOWN,
+      56             :   ODOMETRY,
+      57             :   POSE,
+      58             :   POSECOV,
+      59             :   RANGE,
+      60             :   IMU,
+      61             :   RTK_GPS,
+      62             :   POINT,
+      63             :   VECTOR,
+      64             :   QUAT,
+      65             : } MessageType_t;
+      66             : const int n_MessageType_t = 10;
+      67             : 
+      68             : const std::map<std::string, MessageType_t> map_msg_type{{"nav_msgs/Odometry", MessageType_t::ODOMETRY},
+      69             :                                                         {"geometry_msgs/PoseStamped", MessageType_t::POSE},
+      70             :                                                         {"geometry_msgs/PoseWithCovarianceStamped", MessageType_t::POSECOV},
+      71             :                                                         {"sensor_msgs/Range", MessageType_t::RANGE},
+      72             :                                                         {"sensor_msgs/Imu", MessageType_t::IMU},
+      73             :                                                         {"mrs_msgs/RtkGps", MessageType_t::RTK_GPS},
+      74             :                                                         {"geometry_msgs/PointStamped", MessageType_t::POINT},
+      75             :                                                         {"geometry_msgs/Vector3Stamped", MessageType_t::VECTOR},
+      76             :                                                         {"geometry_msgs/QuaternionStamped", MessageType_t::QUAT}};
+      77             : 
+      78             : template <int n_measurements>
+      79             : class Correction {
+      80             : 
+      81             :   using CommonHandlers_t  = mrs_uav_managers::estimation_manager::CommonHandlers_t;
+      82             :   using PrivateHandlers_t = mrs_uav_managers::estimation_manager::PrivateHandlers_t;
+      83             :   using StateId_t         = mrs_uav_managers::estimation_manager::StateId_t;
+      84             : 
+      85             : public:
+      86             :   typedef Eigen::Matrix<double, n_measurements, 1>         measurement_t;
+      87             :   typedef mrs_lib::DynamicReconfigureMgr<CorrectionConfig> drmgr_t;
+      88             : 
+      89             :   struct MeasurementStamped
+      90             :   {
+      91             :     ros::Time     stamp;
+      92             :     measurement_t value;
+      93             :   };
+      94             : 
+      95             : public:
+      96             :   Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& frame_id, const EstimatorType_t& est_type,
+      97             :              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
+      98             :              std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction);
+      99             : 
+     100             :   std::string getName() const;
+     101             :   std::string getNamespacedName() const;
+     102             :   std::string getPrintName() const;
+     103             : 
+     104             :   double    getR();
+     105             :   void      setR(const double R);
+     106             :   StateId_t getStateId() const;
+     107             : 
+     108             :   bool             isHealthy();
+     109             :   ros::Time        healthy_time_;
+     110             :   std::atomic_bool is_healthy_    = true;
+     111             :   std::atomic_bool is_delay_ok_   = true;
+     112             :   std::atomic_bool is_dt_ok_      = true;
+     113             :   std::atomic_bool is_nan_free_   = true;
+     114             :   std::atomic_bool got_first_msg_ = false;
+     115             : 
+     116             :   int counter_nan_ = 0;
+     117             : 
+     118             :   std::optional<MeasurementStamped> getRawCorrection();
+     119             :   std::optional<MeasurementStamped> getProcessedCorrection();
+     120             : 
+     121             :   void resetProcessors();
+     122             : 
+     123             : private:
+     124             :   std::atomic_bool is_initialized_ = false;
+     125             : 
+     126             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odom_;
+     127             :   void                                          callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     128             :   std::optional<measurement_t>                  getCorrectionFromOdometry(const nav_msgs::OdometryConstPtr msg);
+     129             : 
+     130             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped> sh_pose_s_;
+     131             :   void                                                  callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr msg);
+     132             :   std::optional<measurement_t>                          getCorrectionFromPoseStamped(const geometry_msgs::PoseStampedConstPtr msg);
+     133             : 
+     134             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped> sh_pose_wcs_;
+     135             : 
+     136             :   mrs_lib::SubscribeHandler<mrs_msgs::RtkGps> sh_rtk_;
+     137             :   void                                        callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg);
+     138             :   std::optional<measurement_t>                getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg);
+     139             :   void                                        getAvgRtkInitZ(const double rtk_z);
+     140             :   bool                                        got_avg_init_rtk_z_ = false;
+     141             :   double                                      rtk_init_z_avg_     = 0.0;
+     142             :   int                                         got_rtk_counter_    = 0;
+     143             : 
+     144             :   mrs_lib::SubscribeHandler<geometry_msgs::PointStamped> sh_point_;
+     145             :   void                                                   callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg);
+     146             :   std::optional<measurement_t>                           getCorrectionFromPoint(const geometry_msgs::PointStampedConstPtr msg);
+     147             : 
+     148             :   mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped> sh_vector_;
+     149             :   std::optional<measurement_t>                             getCorrectionFromVector(const geometry_msgs::Vector3StampedConstPtr msg);
+     150             :   void                                                     callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg);
+     151             : 
+     152             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_;  // for obtaining heading rate
+     153             :   std::string                                                 orientation_topic_;
+     154             : 
+     155             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_quat_;
+     156             :   measurement_t                                               prev_hdg_measurement_;
+     157             :   bool                                                        got_first_hdg_measurement_ = false;
+     158             : 
+     159             :   mrs_lib::SubscribeHandler<sensor_msgs::Imu> sh_imu_;
+     160             :   std::optional<measurement_t>                             getCorrectionFromImu(const sensor_msgs::ImuConstPtr msg);
+     161             :   void                                                     callbackImu(const sensor_msgs::Imu::ConstPtr msg);
+     162             : 
+     163             :   mrs_lib::SubscribeHandler<sensor_msgs::Range> sh_range_;
+     164             :   std::optional<measurement_t>                  getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg);
+     165             :   void                                          callbackRange(const sensor_msgs::Range::ConstPtr msg);
+     166             :   ros::ServiceServer                            ser_toggle_range_;
+     167             :   bool                                          callbackToggleRange(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     168             :   bool                                          range_enabled_ = true;
+     169             : 
+     170             :   void applyCorrection(const measurement_t& meas, const ros::Time& stamp);
+     171             : 
+     172             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_raw_;
+     173             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_proc_;
+     174             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>      ph_delay_;
+     175             : 
+     176             :   const std::string                  est_name_;
+     177             :   const std::string                  name_;
+     178             :   const std::string                  ns_frame_id_;
+     179             :   const EstimatorType_t              est_type_;
+     180             :   std::shared_ptr<CommonHandlers_t>  ch_;
+     181             :   std::shared_ptr<PrivateHandlers_t> ph_;
+     182             : 
+     183             :   MessageType_t msg_type_;
+     184             :   std::string   msg_topic_;
+     185             :   double        msg_timeout_;
+     186             : 
+     187             :   double     R_;
+     188             :   double     default_R_;
+     189             :   double     R_coeff_;
+     190             :   std::mutex mtx_R_;
+     191             :   StateId_t  state_id_;
+     192             :   bool       is_in_body_frame_ = true;
+     193             :   double     gravity_norm_ = 9.8066;
+     194             : 
+     195             :   std::unique_ptr<drmgr_t> drmgr_;
+     196             : 
+     197             :   std::optional<measurement_t> getCorrectionFromQuat(const geometry_msgs::QuaternionStampedConstPtr msg);
+     198             :   std::optional<measurement_t> getZVelUntilted(const geometry_msgs::Vector3& msg, const std_msgs::Header& header);
+     199             :   std::optional<measurement_t> getVecInFrame(const geometry_msgs::Vector3& vec_in, const std_msgs::Header& source_header, const std::string target_frame);
+     200             : 
+     201             :   std::optional<geometry_msgs::Pose> transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const;
+     202             : 
+     203             :   void   checkMsgDelay(const ros::Time& msg_time);
+     204             :   double msg_delay_limit_;
+     205             :   double msg_delay_warn_limit_;
+     206             : 
+     207             :   double time_since_last_msg_limit_;
+     208             : 
+     209             :   std::shared_ptr<Processor<n_measurements>> createProcessorFromName(const std::string& name, ros::NodeHandle& nh);
+     210             :   bool                                       process(measurement_t& measurement);
+     211             : 
+     212             :   bool             isTimestampOk();
+     213             :   bool             isMsgComing();
+     214             :   std::atomic_bool first_timestamp_ = true;
+     215             :   ros::Time        msg_time_;
+     216             :   ros::Time        prev_msg_time_;
+     217             :   std::mutex       mtx_msg_time_;
+     218             : 
+     219             :   std::vector<std::string>                                                    processor_names_;
+     220             :   std::unordered_map<std::string, std::shared_ptr<Processor<n_measurements>>> processors_;
+     221             : 
+     222             :   std::function<double(int, int)>                            fun_get_state_;
+     223             :   std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction_;
+     224             : 
+     225             :   void publishCorrection(const MeasurementStamped& measurement_stamped, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr);
+     226             :   void publishDelay(const double delay);
+     227             : };
+     228             : 
+     229             : /*//{ constructor */
+     230             : template <int n_measurements>
+     231         354 : Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& ns_frame_id,
+     232             :                                        const EstimatorType_t& est_type, const std::shared_ptr<CommonHandlers_t>& ch,
+     233             :                                        const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
+     234             :                                        std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction)
+     235             :     : est_name_(est_name),
+     236             :       name_(name),
+     237             :       ns_frame_id_(ns_frame_id),
+     238             :       est_type_(est_type),
+     239             :       ch_(ch),
+     240             :       ph_(ph),
+     241             :       fun_get_state_(fun_get_state),
+     242         354 :       fun_apply_correction_(fun_apply_correction) {
+     243             : 
+     244             :   // | --------------------- load parameters -------------------- |
+     245             : 
+     246         708 :   std::string msg_type_string;
+     247             : 
+     248         354 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + getNamespacedName() + "/");
+     249             : 
+     250         354 :   ph->param_loader->loadParam("message/type", msg_type_string);
+     251         354 :   if (map_msg_type.find(msg_type_string) == map_msg_type.end()) {
+     252           0 :     ROS_ERROR("[%s]: wrong message type: %s of correction %s", getPrintName().c_str(), msg_type_string.c_str(), getName().c_str());
+     253           0 :     ros::shutdown();
+     254             :   }
+     255         354 :   msg_type_ = map_msg_type.at(msg_type_string);
+     256             : 
+     257         354 :   ph->param_loader->loadParam("message/topic", msg_topic_);
+     258         354 :   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
+     259         354 :   ph->param_loader->loadParam("message/limit/delay", msg_delay_limit_);
+     260         354 :   msg_delay_warn_limit_ = msg_delay_limit_ / 2;  // maybe specify this as a param?
+     261         354 :   ph->param_loader->loadParam("message/limit/time_since_last", time_since_last_msg_limit_);
+     262             : 
+     263             :   int state_id_tmp;
+     264         354 :   ph->param_loader->loadParam("state_id", state_id_tmp);
+     265         354 :   if (state_id_tmp < n_StateId_t) {
+     266         354 :     state_id_ = static_cast<StateId_t>(state_id_tmp);
+     267             :   } else {
+     268           0 :     ROS_ERROR("[%s]: wrong state id: %d of correction %s", getPrintName().c_str(), state_id_tmp, getName().c_str());
+     269           0 :     ros::shutdown();
+     270             :   }
+     271             : 
+     272         354 :   if (state_id_ == StateId_t::VELOCITY) {
+     273         103 :     ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
+     274             :   }
+     275             :   
+     276         354 :   if (state_id_ == StateId_t::ACCELERATION) {
+     277           0 :     ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
+     278           0 :     ph->param_loader->loadParam("gravity_norm", gravity_norm_, 9.8066);
+     279             :   }
+     280             : 
+     281         354 :   ph->param_loader->loadParam("noise", R_);
+     282         354 :   ph->param_loader->loadParam("noise_unhealthy_coeff", R_coeff_);
+     283         354 :   default_R_ = R_;
+     284             : 
+     285             :   // | --------------- processors initialization --------------- |
+     286         354 :   ph->param_loader->loadParam("processors", processor_names_);
+     287             : 
+     288         686 :   for (auto proc_name : processor_names_) {
+     289         332 :     processors_[proc_name] = createProcessorFromName(proc_name, nh);
+     290             :   }
+     291             : 
+     292             :   // | ------------- initialize dynamic reconfigure ------------- |
+     293         354 :   drmgr_               = std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), getPrintName());
+     294         354 :   drmgr_->config.noise = R_;
+     295         354 :   drmgr_->update_config(drmgr_->config);
+     296             : 
+     297             :   // | -------------- initialize subscribe handlers ------------- |
+     298         708 :   mrs_lib::SubscribeHandlerOptions shopts;
+     299         354 :   shopts.nh                 = nh;
+     300         354 :   shopts.node_name          = getPrintName();
+     301         354 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     302         354 :   shopts.threadsafe         = true;
+     303         354 :   shopts.autostart          = true;
+     304         354 :   shopts.queue_size         = 10;
+     305         354 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     306             : 
+     307         354 :   switch (msg_type_) {
+     308           0 :     case MessageType_t::ODOMETRY: {
+     309           0 :       sh_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_, &Correction::callbackOdometry, this);
+     310           0 :       break;
+     311             :     }
+     312           0 :     case MessageType_t::POSE: {
+     313           0 :       sh_pose_s_ = mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped>(shopts, msg_topic_, &Correction::callbackPoseStamped, this);
+     314           0 :       break;
+     315             :     }
+     316           0 :     case MessageType_t::POSECOV: {
+     317             :       // TODO implement
+     318             :       /* sh_pose_wcs_ = mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped>(shopts, msg_topic_); */
+     319           0 :       break;
+     320             :     }
+     321         150 :     case MessageType_t::RANGE: {
+     322         150 :       sh_range_                   = mrs_lib::SubscribeHandler<sensor_msgs::Range>(shopts, msg_topic_, &Correction::callbackRange, this);
+     323         150 :       const std::size_t found     = ros::this_node::getName().find_last_of("/");
+     324         300 :       std::string       node_name = ros::this_node::getName().substr(found + 1);
+     325         300 :       ser_toggle_range_ =
+     326         150 :           nh.advertiseService(ros::this_node::getName() + "/" + getNamespacedName() + "/toggle_range_in", &Correction::callbackToggleRange, this);
+     327         150 :       break;
+     328             :     }
+     329           0 :     case MessageType_t::IMU: {
+     330           0 :       sh_imu_ = mrs_lib::SubscribeHandler<sensor_msgs::Imu>(shopts, msg_topic_, &Correction::callbackImu, this);
+     331           0 :       break;
+     332             :     }
+     333           6 :     case MessageType_t::RTK_GPS: {
+     334           6 :       sh_rtk_ = mrs_lib::SubscribeHandler<mrs_msgs::RtkGps>(shopts, msg_topic_, &Correction::callbackRtk, this);
+     335           6 :       break;
+     336             :     }
+     337          95 :     case MessageType_t::POINT: {
+     338          95 :       sh_point_ = mrs_lib::SubscribeHandler<geometry_msgs::PointStamped>(shopts, msg_topic_, &Correction::callbackPoint, this);
+     339          95 :       break;
+     340             :     }
+     341         103 :     case MessageType_t::VECTOR: {
+     342         103 :       sh_vector_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, msg_topic_, &Correction::callbackVector, this);
+     343         103 :       break;
+     344             :     }
+     345           0 :     case MessageType_t::QUAT: {
+     346           0 :       sh_quat_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, msg_topic_);
+     347           0 :       break;
+     348             :     }
+     349           0 :     case MessageType_t::UNKNOWN: {
+     350           0 :       ROS_ERROR("[%s]: UNKNOWN message type of correction", getPrintName().c_str());
+     351           0 :       break;
+     352             :     }
+     353           0 :     default: {
+     354           0 :       ROS_ERROR("[%s]: unhandled message type", getPrintName().c_str());
+     355             :     }
+     356             :   }
+     357             : 
+     358             :   // | ------ subscribe orientation for obtaingin hdg rate ------ |
+     359         354 :   if (est_type_ == EstimatorType_t::HEADING && state_id_ == StateId_t::VELOCITY) {
+     360           0 :     ph->param_loader->loadParam("message/orientation_topic", orientation_topic_);
+     361           0 :     orientation_topic_ = "/" + ch_->uav_name + "/" + orientation_topic_;
+     362           0 :     sh_orientation_    = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, orientation_topic_);
+     363             :   }
+     364             : 
+     365             : 
+     366             :   // | --------------- initialize publish handlers -------------- |
+     367         354 :   if (ch_->debug_topics.correction) {
+     368         354 :     ph_correction_raw_  = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/raw", 10);
+     369         354 :     ph_correction_proc_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/proc", 10);
+     370             :   }
+     371         354 :   if (ch_->debug_topics.corr_delay) {
+     372           0 :     ph_delay_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, est_name_ + "/correction/" + getName() + "/delay", 10);
+     373             :   }
+     374             : 
+     375             :   // | --- check whether all parameters were loaded correctly --- |
+     376         354 :   if (!ph->param_loader->loadedSuccessfully()) {
+     377           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     378           0 :     ros::shutdown();
+     379             :   }
+     380             : 
+     381         354 :   healthy_time_ = ros::Time(0);
+     382             : 
+     383         354 :   is_initialized_ = true;
+     384         354 : }
+     385             : /*//}*/
+     386             : 
+     387             : /*//{ getName() */
+     388             : template <int n_measurements>
+     389         708 : std::string Correction<n_measurements>::getName() const {
+     390         708 :   return name_;
+     391             : }
+     392             : /*//}*/
+     393             : 
+     394             : /*//{ getNamespacedName() */
+     395             : template <int n_measurements>
+     396        1431 : std::string Correction<n_measurements>::getNamespacedName() const {
+     397        1431 :   return est_name_ + "/" + name_;
+     398             : }
+     399             : /*//}*/
+     400             : 
+     401             : /*//{ getPrintName() */
+     402             : template <int n_measurements>
+     403         778 : std::string Correction<n_measurements>::getPrintName() const {
+     404         778 :   return ch_->nodelet_name + "/" + est_name_ + "/" + name_;
+     405             : }
+     406             : /*//}*/
+     407             : 
+     408             : /*//{ getR() */
+     409             : template <int n_measurements>
+     410     2394024 : double Correction<n_measurements>::getR() {
+     411     2394024 :   std::scoped_lock lock(mtx_R_);
+     412     2393608 :   default_R_ = drmgr_->config.noise;
+     413     4786645 :   return R_;
+     414             : }
+     415             : /*//}*/
+     416             : 
+     417             : /*//{ setR() */
+     418             : template <int n_measurements>
+     419      658385 : void Correction<n_measurements>::setR(const double R) {
+     420      658385 :   std::scoped_lock lock(mtx_R_);
+     421      658290 :   R_ = R;
+     422      658291 : }
+     423             : /*//}*/
+     424             : 
+     425             : /*//{ getStateId() */
+     426             : template <int n_measurements>
+     427      658615 : StateId_t Correction<n_measurements>::getStateId() const {
+     428      658615 :   return state_id_;
+     429             : }
+     430             : /*//}*/
+     431             : 
+     432             : /*//{ isHealthy() */
+     433             : template <int n_measurements>
+     434      674657 : bool Correction<n_measurements>::isHealthy() {
+     435             : 
+     436      674657 :   if (!is_initialized_) {
+     437           0 :     return false;
+     438             :   }
+     439             : 
+     440      674341 :   is_dt_ok_ = isMsgComing();
+     441             : 
+     442      674575 :   if (!is_delay_ok_) {
+     443           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: delay not ok", getPrintName().c_str());
+     444             :   }
+     445             : 
+     446      674442 :   if (!is_healthy_) {
+     447           0 :     if (is_dt_ok_ && is_delay_ok_) {
+     448           0 :       if (healthy_time_ > ros::Time(10)) {
+     449           0 :         is_healthy_ = true;
+     450             :       }
+     451             :     } else {
+     452           0 :       healthy_time_ = ros::Time(0);
+     453             :     }
+     454             :   }
+     455             : 
+     456      674388 :   is_healthy_ = is_healthy_ && is_dt_ok_ && is_delay_ok_;
+     457             : 
+     458      674675 :   return is_healthy_;
+     459             : }
+     460             : /*//}*/
+     461             : 
+     462             : /*//{ getRawCorrection() */
+     463             : template <int n_measurements>
+     464       14950 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getRawCorrection() {
+     465             : 
+     466       14950 :   if (!is_initialized_) {
+     467           0 :     return {};
+     468             :   }
+     469             : 
+     470       14933 :   MeasurementStamped measurement_stamped;
+     471             : 
+     472       14925 :   switch (msg_type_) {
+     473             : 
+     474           0 :     case MessageType_t::ODOMETRY: {
+     475             : 
+     476           0 :       if (!sh_odom_.hasMsg()) {
+     477           0 :         return {};
+     478             :       }
+     479             : 
+     480           0 :       auto msg                  = sh_odom_.getMsg();
+     481           0 :       measurement_stamped.stamp = msg->header.stamp;
+     482             :       /* if (!isTimestampOk(measurement_stamped.stamp)) { */
+     483             :       /*   return {}; */
+     484             :       /* } */
+     485             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     486             : 
+     487             :       /* if (!is_delay_ok_) { */
+     488             :       /*   return {}; */
+     489             :       /* } */
+     490           0 :       auto res = getCorrectionFromOdometry(msg);
+     491           0 :       if (res) {
+     492           0 :         measurement_stamped.value = res.value();
+     493             :       } else {
+     494           0 :         return {};
+     495             :       }
+     496           0 :       break;
+     497             :     }
+     498             : 
+     499           0 :     case MessageType_t::POSE: {
+     500             : 
+     501           0 :       if (!sh_pose_s_.hasMsg()) {
+     502           0 :         return {};
+     503             :       }
+     504             : 
+     505           0 :       auto msg                  = sh_pose_s_.getMsg();
+     506           0 :       measurement_stamped.stamp = msg->header.stamp;
+     507             :       /* if (!isTimestampOk(measurement_stamped.stamp)) { */
+     508             :       /*   return {}; */
+     509             :       /* } */
+     510             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     511             : 
+     512             :       /* if (!is_delay_ok_) { */
+     513             :       /*   return {}; */
+     514             :       /* } */
+     515           0 :       auto res = getCorrectionFromPoseStamped(msg);
+     516           0 :       if (res) {
+     517           0 :         measurement_stamped.value = res.value();
+     518             :       } else {
+     519           0 :         return {};
+     520             :       }
+     521           0 :       break;
+     522             :     }
+     523             : 
+     524           0 :     case MessageType_t::POSECOV: {
+     525             :       // TODO implement
+     526             :       /* return getCorrectionFromPoseWCS(msg); */
+     527           0 :       is_healthy_ = false;
+     528           0 :       return {};
+     529             :       break;
+     530             :     }
+     531             : 
+     532        7860 :     case MessageType_t::RANGE: {
+     533             : 
+     534        7860 :       if (!range_enabled_) {
+     535           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: fusing range corrections is disabled", getPrintName().c_str());
+     536        4027 :         return {};
+     537             :       }
+     538             : 
+     539        7860 :       if (!sh_range_.hasMsg()) {
+     540        2939 :         return {};
+     541             :       }
+     542             : 
+     543        4916 :       auto msg                  = sh_range_.getMsg();
+     544        4915 :       measurement_stamped.stamp = msg->header.stamp;
+     545             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     546             : 
+     547             :       /* if (!is_delay_ok_) { */
+     548             :       /*   return {}; */
+     549             :       /* } */
+     550             : 
+     551        4910 :       auto res = getCorrectionFromRange(msg);
+     552        4918 :       if (res) {
+     553        3842 :         measurement_stamped.value = res.value();
+     554             :       } else {
+     555        1076 :         return {};
+     556             :       }
+     557        3842 :       break;
+     558             :     }
+     559             : 
+     560           0 :     case MessageType_t::IMU: {
+     561             : 
+     562           0 :       if (!sh_imu_.hasMsg()) {
+     563           0 :         ROS_ERROR_THROTTLE(1.0, " no imu msgs so far");
+     564           0 :         return {};
+     565             :       }
+     566             : 
+     567           0 :       auto msg                  = sh_imu_.getMsg();
+     568           0 :       measurement_stamped.stamp = msg->header.stamp;
+     569             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     570             : 
+     571             :       /* if (!is_delay_ok_) { */
+     572             :       /*   ROS_ERROR("[%s]: rtk msg delay not ok", ros::this_node::getName().c_str()); */
+     573             :       /*   return {}; */
+     574             :       /* } */
+     575             : 
+     576           0 :       auto res = getCorrectionFromImu(msg);
+     577           0 :       if (res) {
+     578           0 :         measurement_stamped.value = res.value();
+     579             :       } else {
+     580           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not get rtk correction", ros::this_node::getName().c_str());
+     581           0 :         return {};
+     582             :       }
+     583             : 
+     584           0 :       break;
+     585             :     }
+     586             : 
+     587         108 :     case MessageType_t::RTK_GPS: {
+     588             : 
+     589         108 :       if (!sh_rtk_.hasMsg()) {
+     590           0 :         ROS_ERROR_THROTTLE(1.0, " no rtk msgs so far");
+     591          16 :         return {};
+     592             :       }
+     593             : 
+     594         108 :       auto msg                  = sh_rtk_.getMsg();
+     595         108 :       measurement_stamped.stamp = msg->header.stamp;
+     596             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     597             : 
+     598             :       /* if (!is_delay_ok_) { */
+     599             :       /*   ROS_ERROR("[%s]: rtk msg delay not ok", ros::this_node::getName().c_str()); */
+     600             :       /*   return {}; */
+     601             :       /* } */
+     602             : 
+     603         108 :       auto res = getCorrectionFromRtk(msg);
+     604         108 :       if (res) {
+     605          92 :         measurement_stamped.value = res.value();
+     606             :       } else {
+     607          16 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not get rtk correction", ros::this_node::getName().c_str());
+     608          16 :         return {};
+     609             :       }
+     610             : 
+     611          92 :       break;
+     612             :     }
+     613             : 
+     614        6455 :     case MessageType_t::POINT: {
+     615             : 
+     616        6455 :       if (!sh_point_.hasMsg()) {
+     617        2702 :         return {};
+     618             :       }
+     619             : 
+     620        3753 :       auto msg                  = sh_point_.getMsg();
+     621        3753 :       measurement_stamped.stamp = msg->header.stamp;
+     622             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     623             : 
+     624             :       /* if (!is_delay_ok_) { */
+     625             :       /*   return {}; */
+     626             :       /* } */
+     627        3753 :       auto res = getCorrectionFromPoint(msg);
+     628        3753 :       if (res) {
+     629        3753 :         measurement_stamped.value = res.value();
+     630             :       } else {
+     631           0 :         return {};
+     632             :       }
+     633        3753 :       break;
+     634             :     }
+     635             : 
+     636         522 :     case MessageType_t::VECTOR: {
+     637             : 
+     638         522 :       if (!sh_vector_.hasMsg()) {
+     639         419 :         return {};
+     640             :       }
+     641             : 
+     642         126 :       auto msg                  = sh_vector_.getMsg();
+     643         126 :       measurement_stamped.stamp = msg->header.stamp;
+     644             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     645             : 
+     646             :       /* if (!is_delay_ok_) { */
+     647             :       /*   return {}; */
+     648             :       /* } */
+     649         126 :       auto res = getCorrectionFromVector(msg);
+     650         126 :       if (res) {
+     651         103 :         measurement_stamped.value = res.value();
+     652             :       } else {
+     653          23 :         return {};
+     654             :       }
+     655         103 :       break;
+     656             :     }
+     657             : 
+     658           0 :     case MessageType_t::QUAT: {
+     659             : 
+     660           0 :       if (!sh_quat_.newMsg()) {
+     661           0 :         return {};
+     662             :       }
+     663             : 
+     664           0 :       auto msg                  = sh_quat_.getMsg();
+     665           0 :       measurement_stamped.stamp = msg->header.stamp;
+     666             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     667             : 
+     668             :       /* if (!is_delay_ok_) { */
+     669             :       /*   return {}; */
+     670             :       /* } */
+     671           0 :       auto res = getCorrectionFromQuat(msg);
+     672           0 :       if (res) {
+     673           0 :         measurement_stamped.value = res.value();
+     674             :       } else {
+     675           0 :         return {};
+     676             :       }
+     677           0 :       break;
+     678             :     }
+     679             : 
+     680           1 :     default: {
+     681           1 :       ROS_ERROR_THROTTLE(1.0, "[%s]: this type of correction is not implemented in getCorrectionFromMessage()", getPrintName().c_str());
+     682           0 :       is_healthy_ = false;
+     683           0 :       return {};
+     684             :     }
+     685             :   }
+     686             : 
+     687             :   // check for nans
+     688        7790 :   is_nan_free_ = true;
+     689       19411 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+     690       11621 :     if (!std::isfinite(measurement_stamped.value(i))) {
+     691           1 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in correction. Total NaNs: %d", getPrintName().c_str(), ++counter_nan_);
+     692           0 :       is_nan_free_ = false;
+     693           0 :       return {};
+     694             :     }
+     695             :   }
+     696             : 
+     697        7789 :   got_first_msg_ = true;
+     698        7790 :   publishCorrection(measurement_stamped, ph_correction_raw_);
+     699             : 
+     700        7790 :   return measurement_stamped;
+     701             : }
+     702             : /*//}*/
+     703             : 
+     704             : /*//{ getProcessedCorrection() */
+     705             : template <int n_measurements>
+     706       14950 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getProcessedCorrection() {
+     707             : 
+     708       14950 :   MeasurementStamped measurement_stamped;
+     709       14937 :   auto               res = getRawCorrection();
+     710       14943 :   if (res) {
+     711        7790 :     MeasurementStamped measurement_stamped = res.value();
+     712        7787 :     if (process(measurement_stamped.value)) {
+     713         440 :       publishCorrection(measurement_stamped, ph_correction_proc_);
+     714         440 :       return measurement_stamped;
+     715             :     } else {
+     716        7350 :       return {};  // invalid correction
+     717             :     }
+     718             :   } else {
+     719        7157 :     return {};  // invalid correction
+     720             :   }
+     721             : }  // namespace mrs_uav_state_estimation
+     722             : /*//}*/
+     723             : 
+     724             : /*//{ callbackOdometry() */
+     725             : template <int n_measurements>
+     726           0 : void Correction<n_measurements>::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+     727             : 
+     728           0 :   if (!is_initialized_) {
+     729           0 :     return;
+     730             :   }
+     731             : 
+     732           0 :   auto res = getCorrectionFromOdometry(msg);
+     733           0 :   if (res) {
+     734           0 :     applyCorrection(res.value(), msg->header.stamp);
+     735             :   }
+     736             : }
+     737             : /*//}*/
+     738             : 
+     739             : /*//{ getCorrectionFromOdometry() */
+     740             : template <int n_measurements>
+     741           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromOdometry(const nav_msgs::OdometryConstPtr msg) {
+     742             : 
+     743           0 :   switch (est_type_) {
+     744             : 
+     745             :     // handle lateral estimators
+     746           0 :     case EstimatorType_t::LATERAL: {
+     747             : 
+     748           0 :       switch (state_id_) {
+     749             : 
+     750           0 :         case StateId_t::POSITION: {
+     751           0 :           measurement_t measurement;
+     752           0 :           measurement(0) = msg->pose.pose.position.x;
+     753           0 :           measurement(1) = msg->pose.pose.position.y;
+     754           0 :           return measurement;
+     755             :           break;
+     756             :         }
+     757             : 
+     758           0 :         case StateId_t::VELOCITY: {
+     759           0 :           if (is_in_body_frame_) {
+     760           0 :             std_msgs::Header header = msg->header;
+     761           0 :             header.frame_id         = ch_->frames.ns_fcu;  // message in odometry is published in body frame
+     762           0 :             auto res                = getVecInFrame(msg->twist.twist.linear, header, ns_frame_id_ + "_att_only");
+     763           0 :             if (res) {
+     764           0 :               measurement_t measurement;
+     765           0 :               measurement = res.value();
+     766           0 :               return measurement;
+     767             :             } else {
+     768           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str());
+     769           0 :               return {};
+     770             :             }
+     771             :           } else {
+     772           0 :             measurement_t measurement;
+     773           0 :             measurement(0) = msg->twist.twist.linear.x;
+     774           0 :             measurement(1) = msg->twist.twist.linear.y;
+     775           0 :             return measurement;
+     776             :           }
+     777             :           break;
+     778             :         }
+     779             : 
+     780           0 :         default: {
+     781           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     782           0 :           return {};
+     783             :         }
+     784             :       }
+     785             :       break;
+     786             :     }
+     787             : 
+     788             :     // handle altitude estimators
+     789           0 :     case EstimatorType_t::ALTITUDE: {
+     790             : 
+     791           0 :       switch (state_id_) {
+     792             : 
+     793           0 :         case StateId_t::POSITION: {
+     794           0 :           measurement_t measurement;
+     795           0 :           measurement(0) = msg->pose.pose.position.z;
+     796           0 :           return measurement;
+     797             :           break;
+     798             :         }
+     799             : 
+     800           0 :         case StateId_t::VELOCITY: {
+     801           0 :           if (is_in_body_frame_) {
+     802           0 :             std_msgs::Header header = msg->header;
+     803           0 :             header.frame_id         = ch_->frames.ns_fcu;
+     804           0 :             auto res                = getZVelUntilted(msg->twist.twist.linear, header);
+     805           0 :             if (res) {
+     806           0 :               measurement_t measurement;
+     807           0 :               measurement = res.value();
+     808           0 :               return measurement;
+     809             :             } else {
+     810           0 :               return {};
+     811             :             }
+     812             :           } else {
+     813           0 :             measurement_t measurement;
+     814           0 :             measurement(0) = msg->twist.twist.linear.z;
+     815           0 :             return measurement;
+     816             :           }
+     817             :           break;
+     818             :         }
+     819             : 
+     820           0 :         default: {
+     821           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     822           0 :           return {};
+     823             :         }
+     824             :       }
+     825             :       break;
+     826             :     }
+     827             : 
+     828             :     // handle heading estimators
+     829           0 :     case EstimatorType_t::HEADING: {
+     830             : 
+     831           0 :       switch (state_id_) {
+     832             : 
+     833           0 :         case StateId_t::POSITION: {
+     834           0 :           measurement_t measurement;
+     835             :           try {
+     836             :             // obtain heading from orientation
+     837           0 :             measurement(StateId_t::POSITION) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading();
+     838             :             // unwrap heading wrt previous measurement
+     839           0 :             if (got_first_hdg_measurement_) {
+     840           0 :               measurement(StateId_t::POSITION) = mrs_lib::geometry::radians::unwrap(measurement(POSITION), prev_hdg_measurement_(POSITION));
+     841             :             } else {
+     842           0 :               got_first_hdg_measurement_ = true;
+     843             :             }
+     844           0 :             prev_hdg_measurement_ = measurement;
+     845           0 :             return measurement;
+     846             :           }
+     847           0 :           catch (...) {
+     848           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: failed to obtain heading", getPrintName().c_str());
+     849           0 :             return {};
+     850             :           }
+     851             :           break;
+     852             :         }
+     853             : 
+     854             :           /* case StateId_t::VELOCITY: { */
+     855             :           /*   try { */
+     856             :           /*     measurement_t measurement; */
+     857             :           /*     measurement(0) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeadingRate(msg->twist.twist.angular); */
+     858             :           /*     return measurement; */
+     859             :           /*   } */
+     860             :           /*   catch (...) { */
+     861             :           /*     ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromOdometry())", getPrintName().c_str()); */
+     862             :           /*     return {}; */
+     863             :           /*   } */
+     864             :           /*   break; */
+     865             :           /* } */
+     866             : 
+     867           0 :         default: {
+     868           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     869           0 :           return {};
+     870             :         }
+     871             :       }
+     872             :       break;
+     873             :     }
+     874             :   }
+     875             : 
+     876           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+     877           0 :   return {};
+     878             : }
+     879             : /*//}*/
+     880             : 
+     881             : /*//{ callbackPoseStamped() */
+     882             : template <int n_measurements>
+     883           0 : void Correction<n_measurements>::callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr msg) {
+     884             : 
+     885           0 :   if (!is_initialized_) {
+     886           0 :     return;
+     887             :   }
+     888             : 
+     889           0 :   auto res = getCorrectionFromPoseStamped(msg);
+     890           0 :   if (res) {
+     891           0 :     applyCorrection(res.value(), msg->header.stamp);
+     892             :   }
+     893             : }
+     894             : /*//}*/
+     895             : 
+     896             : /*//{ getCorrectionFromPoseStamped() */
+     897             : template <int n_measurements>
+     898           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromPoseStamped(
+     899             :     const geometry_msgs::PoseStampedConstPtr msg) {
+     900             : 
+     901           0 :   switch (est_type_) {
+     902             : 
+     903             :     // handle lateral estimators
+     904           0 :     case EstimatorType_t::LATERAL: {
+     905             : 
+     906           0 :       switch (state_id_) {
+     907             : 
+     908           0 :         case StateId_t::POSITION: {
+     909           0 :           measurement_t measurement;
+     910           0 :           measurement(0) = msg->pose.position.x;
+     911           0 :           measurement(1) = msg->pose.position.y;
+     912           0 :           return measurement;
+     913             :           break;
+     914             :         }
+     915             : 
+     916           0 :         default: {
+     917           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+     918           0 :           return {};
+     919             :         }
+     920             :       }
+     921             :       break;
+     922             :     }
+     923             : 
+     924             :     // handle altitude estimators
+     925           0 :     case EstimatorType_t::ALTITUDE: {
+     926             : 
+     927           0 :       switch (state_id_) {
+     928             : 
+     929           0 :         case StateId_t::POSITION: {
+     930           0 :           measurement_t measurement;
+     931           0 :           measurement(0) = msg->pose.position.z;
+     932           0 :           return measurement;
+     933             :           break;
+     934             :         }
+     935             : 
+     936           0 :         default: {
+     937           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+     938           0 :           return {};
+     939             :         }
+     940             :       }
+     941             :       break;
+     942             :     }
+     943             : 
+     944             :     // handle heading estimators
+     945           0 :     case EstimatorType_t::HEADING: {
+     946             : 
+     947           0 :       switch (state_id_) {
+     948             : 
+     949           0 :         case StateId_t::POSITION: {
+     950           0 :           measurement_t measurement;
+     951             :           try {
+     952             :             // obtain heading from orientation
+     953           0 :             measurement(StateId_t::POSITION) = mrs_lib::AttitudeConverter(msg->pose.orientation).getHeading();
+     954             :             // unwrap heading wrt previous measurement
+     955           0 :             if (got_first_hdg_measurement_) {
+     956           0 :               measurement(StateId_t::POSITION) = mrs_lib::geometry::radians::unwrap(measurement(POSITION), prev_hdg_measurement_(POSITION));
+     957             :             } else {
+     958           0 :               got_first_hdg_measurement_ = true;
+     959             :             }
+     960           0 :             prev_hdg_measurement_ = measurement;
+     961           0 :             return measurement;
+     962             :           }
+     963           0 :           catch (...) {
+     964           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: failed to obtain heading", getPrintName().c_str());
+     965           0 :             return {};
+     966             :           }
+     967             :           break;
+     968             :         }
+     969             : 
+     970           0 :         default: {
+     971           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+     972           0 :           return {};
+     973             :         }
+     974             :       }
+     975             :       break;
+     976             :     }
+     977             :   }
+     978             : 
+     979           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+     980           0 :   return {};
+     981             : }
+     982             : /*//}*/
+     983             : 
+     984             : /*//{ callbackRange() */
+     985             : template <int n_measurements>
+     986      266160 : void Correction<n_measurements>::callbackRange(const sensor_msgs::Range::ConstPtr msg) {
+     987             : 
+     988      266160 :   if (!is_initialized_) {
+     989           0 :     return;
+     990             :   }
+     991             : 
+     992      264539 :   auto res = getCorrectionFromRange(msg);
+     993      266767 :   if (res) {
+     994      265855 :     applyCorrection(res.value(), msg->header.stamp);
+     995             :   }
+     996             : }
+     997             : /*//}*/
+     998             : 
+     999             : /*//{ getCorrectionFromRange() */
+    1000             : template <int n_measurements>
+    1001      269677 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg) {
+    1002             : 
+    1003      269677 :   if (!range_enabled_) {
+    1004           0 :     ROS_INFO_THROTTLE(1.0, "[%s]: fusing range corrections is disabled", getPrintName().c_str());
+    1005           0 :     return {};
+    1006             :   }
+    1007             : 
+    1008      269677 :   if (!std::isfinite(msg->range)) {
+    1009           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: received value: %f. Not using this correction.", getPrintName().c_str(), msg->range);
+    1010           0 :     return {};
+    1011             :   }
+    1012             : 
+    1013      539397 :   geometry_msgs::PoseStamped range_point;
+    1014             : 
+    1015      269211 :   range_point.header           = msg->header;
+    1016      271004 :   range_point.pose.position.x  = msg->range;
+    1017      268181 :   range_point.pose.position.y  = 0;
+    1018      268181 :   range_point.pose.position.z  = 0;
+    1019      268181 :   range_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1020             : 
+    1021      540743 :   auto res = ch_->transformer->transformSingle(range_point, ch_->frames.ns_fcu_untilted);
+    1022             : 
+    1023      271692 :   Correction::measurement_t measurement;
+    1024             : 
+    1025      271692 :   if (res) {
+    1026      269710 :     measurement(0) = -res.value().pose.position.z;
+    1027      269704 :     return measurement;
+    1028             :   } else {
+    1029        1982 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform range measurement to %s. Not using this correction.", getPrintName().c_str(),
+    1030             :                        ch_->frames.ns_fcu_untilted.c_str());
+    1031        1982 :     return {};
+    1032             :   }
+    1033             : }
+    1034             : /*//}*/
+    1035             : 
+    1036             : /*//{ callbackImu() */
+    1037             : template <int n_measurements>
+    1038           0 : void Correction<n_measurements>::callbackImu(const sensor_msgs::Imu::ConstPtr msg) {
+    1039             : 
+    1040           0 :   if (!is_initialized_) {
+    1041           0 :     return;
+    1042             :   }
+    1043             : 
+    1044           0 :   auto res = getCorrectionFromImu(msg);
+    1045           0 :   if (res) {
+    1046           0 :     applyCorrection(res.value(), msg->header.stamp);
+    1047             :   } else {
+    1048           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from Imu msg", getPrintName().c_str());
+    1049             :   }
+    1050             : }
+    1051             : /*//}*/
+    1052             : 
+    1053             : /*//{ getCorrectionFromImu() */
+    1054             : template <int n_measurements>
+    1055           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromImu(
+    1056             :     const sensor_msgs::ImuConstPtr msg) {
+    1057             : 
+    1058           0 :   switch (est_type_) {
+    1059             : 
+    1060             :     // handle lateral estimators
+    1061           0 :     case EstimatorType_t::LATERAL: {
+    1062             : 
+    1063           0 :       switch (state_id_) {
+    1064             : 
+    1065           0 :         case StateId_t::ACCELERATION: {
+    1066           0 :           if (is_in_body_frame_) {
+    1067           0 :             auto res = getVecInFrame(msg->linear_acceleration, msg->header, ns_frame_id_ + "_att_only");
+    1068           0 :             if (res) {
+    1069           0 :               measurement_t measurement;
+    1070           0 :               measurement = res.value();
+    1071           0 :               return measurement;
+    1072             :             } else {
+    1073           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain IMU acceleration in frame: %s", getPrintName().c_str(), (ns_frame_id_+"_att_only").c_str());
+    1074           0 :               return {};
+    1075             :             }
+    1076             :           } else {
+    1077           0 :             measurement_t measurement;
+    1078           0 :             measurement(0) = msg->linear_acceleration.x;
+    1079           0 :             measurement(1) = msg->linear_acceleration.y;
+    1080           0 :             return measurement;
+    1081             :           }
+    1082             :           break;
+    1083             :         }
+    1084             : 
+    1085           0 :         default: {
+    1086           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str());
+    1087           0 :           return {};
+    1088             :         }
+    1089             :       }
+    1090             :       break;
+    1091             :     }
+    1092             : 
+    1093             :     // handle altitude estimators
+    1094           0 :     case EstimatorType_t::ALTITUDE: {
+    1095             : 
+    1096           0 :       switch (state_id_) {
+    1097             : 
+    1098           0 :         case StateId_t::ACCELERATION: {
+    1099           0 :           if (is_in_body_frame_) {
+    1100           0 :             auto res = getZVelUntilted(msg->linear_acceleration, msg->header);
+    1101           0 :             if (res) {
+    1102           0 :               measurement_t measurement;
+    1103           0 :               measurement = res.value();
+    1104           0 :               measurement(0) -= gravity_norm_;
+    1105           0 :               return measurement;
+    1106             :             } else {
+    1107           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain IMU Z acceleration", getPrintName().c_str());
+    1108           0 :               return {};
+    1109             :             }
+    1110             :           } else {
+    1111           0 :             measurement_t measurement;
+    1112           0 :             measurement(0) = msg->linear_acceleration.z - gravity_norm_;
+    1113           0 :             return measurement;
+    1114             :           }
+    1115             :           break;
+    1116             :         }
+    1117             : 
+    1118           0 :         default: {
+    1119           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str());
+    1120           0 :           return {};
+    1121             :         }
+    1122             :       }
+    1123             :       break;
+    1124             :     }
+    1125             : 
+    1126             :     // handle heading estimators
+    1127           0 :     case EstimatorType_t::HEADING: {
+    1128             : 
+    1129           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::HEADING in getCorrectionFromImu() not implemented", getPrintName().c_str());
+    1130           0 :       return {};
+    1131             :       break;
+    1132             :     }
+    1133             :   }
+    1134             : 
+    1135           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1136           0 :   return {};
+    1137             : }
+    1138             : /*//}*/
+    1139             : 
+    1140             : /*//{ callbackRtk() */
+    1141             : template <int n_measurements>
+    1142        5516 : void Correction<n_measurements>::callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg) {
+    1143             : 
+    1144        5516 :   if (!is_initialized_) {
+    1145           0 :     return;
+    1146             :   }
+    1147             : 
+    1148        5509 :   auto res = getCorrectionFromRtk(msg);
+    1149        5525 :   if (res) {
+    1150        5519 :     applyCorrection(res.value(), msg->header.stamp);
+    1151             :   }
+    1152             : }
+    1153             : /*//}*/
+    1154             : 
+    1155             : /*//{ getCorrectionFromRtk() */
+    1156             : template <int n_measurements>
+    1157        5629 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg) {
+    1158             : 
+    1159       11262 :   geometry_msgs::PoseStamped rtk_pos;
+    1160             : 
+    1161        5622 :   if (!std::isfinite(msg->gps.latitude)) {
+    1162           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->latitude\"!!!", getPrintName().c_str());
+    1163           0 :     return {};
+    1164             :   }
+    1165             : 
+    1166        5624 :   if (!std::isfinite(msg->gps.longitude)) {
+    1167           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->longitude\"!!!", getPrintName().c_str());
+    1168           0 :     return {};
+    1169             :   }
+    1170             : 
+    1171        5621 :   if (!std::isfinite(msg->gps.altitude)) {
+    1172           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->altitude\"!!!", getPrintName().c_str());
+    1173           0 :     return {};
+    1174             :   }
+    1175             : 
+    1176        5617 :   if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+    1177           0 :     ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+    1178           0 :     return {};
+    1179             :   }
+    1180             : 
+    1181        5624 :   rtk_pos.header = msg->header;
+    1182        5626 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+    1183        5607 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+    1184        5610 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1185             : 
+    1186        5624 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+    1187        5628 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+    1188             : 
+    1189        5625 :   Correction::measurement_t measurement;
+    1190             : 
+    1191             :   // transform the RTK position from antenna to FCU
+    1192        5627 :   auto res = transformRtkToFcu(rtk_pos);
+    1193        5632 :   if (res) {
+    1194        5633 :     rtk_pos.pose = res.value();
+    1195             :   } else {
+    1196           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+    1197           0 :     return {};
+    1198             :   }
+    1199             : 
+    1200        5633 :   switch (est_type_) {
+    1201             : 
+    1202             :     // handle lateral estimators
+    1203        3830 :     case EstimatorType_t::LATERAL: {
+    1204             : 
+    1205        3830 :       switch (state_id_) {
+    1206             : 
+    1207        3830 :         case StateId_t::POSITION: {
+    1208        3830 :           measurement(0) = rtk_pos.pose.position.x;
+    1209        3830 :           measurement(1) = rtk_pos.pose.position.y;
+    1210        3830 :           return measurement;
+    1211             :           break;
+    1212             :         }
+    1213             : 
+    1214           0 :         default: {
+    1215           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromRtk() switch", getPrintName().c_str());
+    1216           0 :           return {};
+    1217             :         }
+    1218             :       }
+    1219             :       break;
+    1220             :     }
+    1221             : 
+    1222             :     // handle altitude estimators
+    1223        1803 :     case EstimatorType_t::ALTITUDE: {
+    1224             : 
+    1225        1803 :       switch (state_id_) {
+    1226             : 
+    1227        1803 :         case StateId_t::POSITION: {
+    1228        1803 :           measurement(0) = rtk_pos.pose.position.z;
+    1229        1803 :           if (!got_avg_init_rtk_z_) {
+    1230          22 :             getAvgRtkInitZ(measurement(0));
+    1231          22 :             return {};
+    1232             :           }
+    1233        1781 :           measurement(0) -= rtk_init_z_avg_;
+    1234        1781 :           return measurement;
+    1235             :           break;
+    1236             :         }
+    1237             : 
+    1238           0 :         default: {
+    1239           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromRtk() switch", getPrintName().c_str());
+    1240           0 :           return {};
+    1241             :         }
+    1242             :       }
+    1243             :       break;
+    1244             :     }
+    1245             : 
+    1246           0 :     case EstimatorType_t::HEADING: {
+    1247           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: should not be possible to get into this branch of getCorrectionFromRtk() switch", getPrintName().c_str());
+    1248           0 :       return {};
+    1249             :       break;
+    1250             :     }
+    1251             :   }
+    1252             : 
+    1253           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1254           0 :   return {};
+    1255             : }
+    1256             : /*//}*/
+    1257             : 
+    1258             : /*//{ callbackPoint() */
+    1259             : template <int n_measurements>
+    1260      187672 : void Correction<n_measurements>::callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg) {
+    1261             : 
+    1262      187672 :   if (!is_initialized_) {
+    1263           0 :     return;
+    1264             :   }
+    1265             : 
+    1266      187493 :   auto res = getCorrectionFromPoint(msg);
+    1267      187504 :   if (res) {
+    1268      187465 :     applyCorrection(res.value(), msg->header.stamp);
+    1269             :   }
+    1270             : }
+    1271             : /*//}*/
+    1272             : 
+    1273             : /*//{ getCorrectionFromPoint() */
+    1274             : template <int n_measurements>
+    1275      191442 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromPoint(
+    1276             :     const geometry_msgs::PointStampedConstPtr msg) {
+    1277             : 
+    1278      191442 :   switch (est_type_) {
+    1279             : 
+    1280             :     // handle lateral estimators
+    1281      191445 :     case EstimatorType_t::LATERAL: {
+    1282             : 
+    1283      191445 :       switch (state_id_) {
+    1284             : 
+    1285      191433 :         case StateId_t::POSITION: {
+    1286      191433 :           measurement_t measurement;
+    1287      191412 :           measurement(0) = msg->point.x;
+    1288      191272 :           measurement(1) = msg->point.y;
+    1289      191284 :           return measurement;
+    1290             :           break;
+    1291             :         }
+    1292             : 
+    1293          12 :         default: {
+    1294          12 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1295           0 :           return {};
+    1296             :         }
+    1297             :       }
+    1298             :       break;
+    1299             :     }
+    1300             : 
+    1301             :     // handle altitude estimators
+    1302           0 :     case EstimatorType_t::ALTITUDE: {
+    1303             : 
+    1304           0 :       switch (state_id_) {
+    1305             : 
+    1306           0 :         case StateId_t::POSITION: {
+    1307           0 :           measurement_t measurement;
+    1308           0 :           measurement(0) = msg->point.z;
+    1309           0 :           return measurement;
+    1310             :           break;
+    1311             :         }
+    1312             : 
+    1313           0 :         default: {
+    1314           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1315           0 :           return {};
+    1316             :         }
+    1317             :       }
+    1318             :       break;
+    1319             :     }
+    1320             : 
+    1321           0 :     default: {
+    1322           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1323           0 :       return {};
+    1324             :     }
+    1325             :   }
+    1326             : 
+    1327             : 
+    1328             :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1329             :   return {};
+    1330             : }
+    1331             : /*//}*/
+    1332             : 
+    1333             : /*//{ callbackVector() */
+    1334             : template <int n_measurements>
+    1335      206475 : void Correction<n_measurements>::callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+    1336             : 
+    1337      206475 :   if (!is_initialized_) {
+    1338           0 :     return;
+    1339             :   }
+    1340             : 
+    1341      206295 :   auto res = getCorrectionFromVector(msg);
+    1342      206543 :   if (res) {
+    1343      206511 :     applyCorrection(res.value(), msg->header.stamp);
+    1344             :   } else {
+    1345          30 :     ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from Vector3Stamped msg", getPrintName().c_str());
+    1346             :   }
+    1347             : }
+    1348             : /*//}*/
+    1349             : 
+    1350             : /*//{ getCorrectionFromVector() */
+    1351             : template <int n_measurements>
+    1352      206569 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromVector(
+    1353             :     const geometry_msgs::Vector3StampedConstPtr msg) {
+    1354             : 
+    1355      206569 :   switch (est_type_) {
+    1356             : 
+    1357             :     // handle lateral estimators
+    1358       10857 :     case EstimatorType_t::LATERAL: {
+    1359             : 
+    1360       10857 :       switch (state_id_) {
+    1361             : 
+    1362       10857 :         case StateId_t::VELOCITY: {
+    1363       10857 :           auto res = getVecInFrame(msg->vector, msg->header, ns_frame_id_ + "_att_only");
+    1364       10856 :           if (res) {
+    1365       10812 :             measurement_t measurement;
+    1366       10812 :             measurement = res.value();
+    1367       10811 :             return measurement;
+    1368             :           } else {
+    1369          45 :             return {};
+    1370             :           }
+    1371             :           break;
+    1372             :         }
+    1373             : 
+    1374           0 :         default: {
+    1375           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1376           0 :           return {};
+    1377             :         }
+    1378             :       }
+    1379             :       break;
+    1380             :     }
+    1381             : 
+    1382             :     // handle altitude estimators
+    1383      195747 :     case EstimatorType_t::ALTITUDE: {
+    1384             : 
+    1385      195747 :       switch (state_id_) {
+    1386             : 
+    1387      195677 :         case StateId_t::VELOCITY: {
+    1388      195677 :           auto res = getZVelUntilted(msg->vector, msg->header);
+    1389      195813 :           if (res) {
+    1390      195804 :             measurement_t measurement;
+    1391      195804 :             measurement = res.value();
+    1392      195804 :             return measurement;
+    1393             :           } else {
+    1394           8 :             ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain untilted Z velocity", getPrintName().c_str());
+    1395           8 :             return {};
+    1396             :           }
+    1397             :           break;
+    1398             :         }
+    1399             : 
+    1400          70 :         default: {
+    1401          70 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1402           0 :           return {};
+    1403             :         }
+    1404             :       }
+    1405             :       break;
+    1406             :     }
+    1407             : 
+    1408             :     // handle heading estimators
+    1409           0 :     case EstimatorType_t::HEADING: {
+    1410             : 
+    1411           0 :       switch (state_id_) {
+    1412             : 
+    1413           0 :         case StateId_t::VELOCITY: {
+    1414             :           try {
+    1415           0 :             if (!sh_orientation_.hasMsg()) {
+    1416           0 :               ROS_INFO_THROTTLE(1.0, "[%s]: %s orientation on topic: %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+    1417             :                                 orientation_topic_.c_str());
+    1418           0 :               return {};
+    1419             :             }
+    1420           0 :             measurement_t measurement;
+    1421           0 :             measurement(0) = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+    1422           0 :             return measurement;
+    1423             :           }
+    1424           0 :           catch (...) {
+    1425           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromVector())", getPrintName().c_str());
+    1426           0 :             return {};
+    1427             :           }
+    1428             :           break;
+    1429             :         }
+    1430             : 
+    1431           0 :         default: {
+    1432           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1433           0 :           return {};
+    1434             :         }
+    1435             :       }
+    1436             :       break;
+    1437             :     }
+    1438             :   }
+    1439             : 
+    1440           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1441           0 :   return {};
+    1442             : }
+    1443             : /*//}*/
+    1444             : 
+    1445             : /*//{ getCorrectionFromQuat() */
+    1446             : template <int n_measurements>
+    1447           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromQuat([
+    1448             :     [maybe_unused]] const geometry_msgs::QuaternionStampedConstPtr msg) {
+    1449             : 
+    1450           0 :   switch (est_type_) {
+    1451             : 
+    1452             :       // handle lateral estimators
+    1453             :       /* case EstimatorType_t::LATERAL: { */
+    1454             : 
+    1455             :       /*   default: { */
+    1456             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1457             :       /*     return {}; */
+    1458             :       /*   } break; */
+    1459             :       /* } */
+    1460             : 
+    1461             :       /* // handle altitude estimators */
+    1462             :       /* case EstimatorType_t::ALTITUDE: { */
+    1463             : 
+    1464             :       /* default: { */
+    1465             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1466             :       /*     return {}; */
+    1467             :       /*   } break; */
+    1468             :       /* } */
+    1469             : 
+    1470             :       /* // handle heading estimators */
+    1471             :       /* case EstimatorType_t::HEADING: { */
+    1472             : 
+    1473             :       /*   switch (state_id_) { */
+    1474             : 
+    1475             :       /*     /1* default: { *1/ */
+    1476             :       /*       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1477             :       /*       return {}; */
+    1478             :       /*     } */
+    1479             :       /*   } */
+    1480             :     default: {
+    1481           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1482           0 :       return {};
+    1483             :     }
+    1484             :   }
+    1485             : 
+    1486             :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1487             :   return {};
+    1488             : }
+    1489             : /*//}*/
+    1490             : 
+    1491             : /*//{ applyCorrection() */
+    1492             : template <int n_measurements>
+    1493      665541 : void Correction<n_measurements>::applyCorrection(const measurement_t& meas, const ros::Time& stamp) {
+    1494             : 
+    1495             :   {
+    1496      665541 :     std::scoped_lock lock(mtx_msg_time_);
+    1497      665415 :     if (first_timestamp_) {
+    1498         353 :       prev_msg_time_   = stamp - ros::Duration(0.01);
+    1499         350 :       msg_time_        = stamp;
+    1500         350 :       healthy_time_    = ros::Time(0);
+    1501         351 :       first_timestamp_ = false;
+    1502             :     }
+    1503             : 
+    1504      665290 :     prev_msg_time_ = msg_time_;
+    1505      665290 :     msg_time_      = stamp;
+    1506      665290 :     healthy_time_ += msg_time_ - prev_msg_time_;
+    1507             :   }
+    1508             : 
+    1509      665341 :   MeasurementStamped meas_stamped;
+    1510      665300 :   meas_stamped.value = meas;
+    1511      665406 :   meas_stamped.stamp = stamp;
+    1512      665406 :   publishCorrection(meas_stamped, ph_correction_raw_);
+    1513      665602 :   if (process(meas_stamped.value)) {
+    1514      657901 :     publishCorrection(meas_stamped, ph_correction_proc_);
+    1515      657996 :     fun_apply_correction_(meas_stamped, getR(), getStateId());
+    1516             :   }
+    1517      664995 : }
+    1518             : /*//}*/
+    1519             : 
+    1520             : /* //{ callbackToggleRange() */
+    1521             : template <int n_measurements>
+    1522           0 : bool Correction<n_measurements>::callbackToggleRange(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1523             : 
+    1524           0 :   if (!is_initialized_) {
+    1525           0 :     return false;
+    1526             :   }
+    1527             : 
+    1528           0 :   if (!range_enabled_ && req.data) {
+    1529           0 :     processors_["saturate"]->toggle(true);
+    1530             :   }
+    1531             : 
+    1532           0 :   range_enabled_ = req.data;
+    1533             : 
+    1534             :   // after enabling range we want to start correcting the altitude slowly
+    1535             : 
+    1536           0 :   res.success = true;
+    1537           0 :   res.message = (range_enabled_ ? "Range enabled" : "Range disabled");
+    1538             : 
+    1539           0 :   if (range_enabled_) {
+    1540             : 
+    1541           0 :     ROS_INFO("[%s]: Range enabled.", getPrintName().c_str());
+    1542             : 
+    1543             :   } else {
+    1544             : 
+    1545           0 :     ROS_INFO("[%s]: Range disabled", getPrintName().c_str());
+    1546             :   }
+    1547             : 
+    1548           0 :   return true;
+    1549             : }
+    1550             : 
+    1551             : //}
+    1552             : 
+    1553             : /*//{ getZVelUntilted() */
+    1554             : template <int n_measurements>
+    1555      195772 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getZVelUntilted(const geometry_msgs::Vector3& msg,
+    1556             :                                                                                                               const std_msgs::Header&       header) {
+    1557             : 
+    1558             :   // untilt the desired vector
+    1559      391585 :   geometry_msgs::PointStamped vel;
+    1560      195701 :   vel.point.x = msg.x;
+    1561      195701 :   vel.point.y = msg.y;
+    1562      195701 :   vel.point.z = msg.z;
+    1563      195701 :   vel.header  = header;
+    1564             :   /* vel.header.frame_id = ch_->frames.ns_fcu; */
+    1565      195787 :   vel.header.stamp = header.stamp;
+    1566             : 
+    1567      391599 :   auto res = ch_->transformer->transformSingle(vel, ch_->frames.ns_fcu_untilted);
+    1568      195813 :   if (res) {
+    1569      195805 :     measurement_t measurement;
+    1570      195805 :     measurement(0) = res.value().point.z;
+    1571      195805 :     return measurement;
+    1572             :   } else {
+    1573           8 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform from %s to %s failed", getPrintName().c_str(), vel.header.frame_id.c_str(), ch_->frames.ns_fcu_untilted.c_str());
+    1574           8 :     return {};
+    1575             :   }
+    1576             : }
+    1577             : /*//}*/
+    1578             : 
+    1579             : /*//{ getVecInFrame() */
+    1580             : template <int n_measurements>
+    1581       10856 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getVecInFrame(const geometry_msgs::Vector3& vec_in,
+    1582             :                                                                                                             const std_msgs::Header&       source_header,
+    1583             :                                                                                                             const std::string             target_frame) {
+    1584             : 
+    1585       10856 :   measurement_t measurement;
+    1586             : 
+    1587       21710 :   geometry_msgs::Vector3Stamped vec;
+    1588       10856 :   vec.header = source_header;
+    1589       10851 :   vec.vector = vec_in;
+    1590             : 
+    1591       21708 :   geometry_msgs::Vector3Stamped transformed_vel;
+    1592       21703 :   auto                          res = ch_->transformer->transformSingle(vec, target_frame);
+    1593       10857 :   if (res) {
+    1594       10812 :     transformed_vel = res.value();
+    1595       10812 :     measurement(0)  = transformed_vel.vector.x;
+    1596       10812 :     measurement(1)  = transformed_vel.vector.y;
+    1597       10812 :     return measurement;
+    1598             :   } else {
+    1599          45 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform of velocity from %s to %s failed.", getPrintName().c_str(), vec.header.frame_id.c_str(), target_frame.c_str());
+    1600          45 :     return {};
+    1601             :   }
+    1602             : }
+    1603             : /*//}*/
+    1604             : 
+    1605             : /*//{ transformRtkToFcu() */
+    1606             : template <int n_measurements>
+    1607        5631 : std::optional<geometry_msgs::Pose> Correction<n_measurements>::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+    1608             : 
+    1609       11264 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+    1610             : 
+    1611             :   // inject current orientation into rtk pose
+    1612       11264 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+    1613        5633 :   if (res1) {
+    1614        5633 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+    1615             :   } else {
+    1616           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(),
+    1617             :                        ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str());
+    1618           0 :     return {};
+    1619             :   }
+    1620             : 
+    1621             :   // invert tf
+    1622        5633 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+    1623       11265 :   geometry_msgs::PoseStamped utm_in_antenna;
+    1624        5633 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+    1625        5633 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+    1626        5633 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+    1627             : 
+    1628             :   // transform to fcu
+    1629       11266 :   geometry_msgs::PoseStamped utm_in_fcu;
+    1630        5633 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+    1631        5633 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+    1632       11266 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+    1633             : 
+    1634        5633 :   if (res2) {
+    1635        5633 :     utm_in_fcu = res2.value();
+    1636             :   } else {
+    1637           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform pose to %s. Not using this correction.", getPrintName().c_str(), ch_->frames.ns_fcu.c_str());
+    1638           0 :     return {};
+    1639             :   }
+    1640             : 
+    1641             :   // invert tf
+    1642        5633 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+    1643        5633 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+    1644             : 
+    1645        5633 :   return fcu_in_utm;
+    1646             : }
+    1647             : /*//}*/
+    1648             : 
+    1649             : /*//{ getAvgRtkInitZ() */
+    1650             : template <int n_measurements>
+    1651          22 : void Correction<n_measurements>::getAvgRtkInitZ(const double rtk_z) {
+    1652             : 
+    1653          22 :   if (!got_avg_init_rtk_z_) {
+    1654             : 
+    1655          22 :     double rtk_avg = rtk_init_z_avg_ / got_rtk_counter_;
+    1656             : 
+    1657          22 :     if (got_rtk_counter_ < 10 || (got_rtk_counter_ < 300 && std::fabs(rtk_z - rtk_avg) > 0.1)) {
+    1658             : 
+    1659          20 :       rtk_init_z_avg_ += rtk_z;
+    1660          20 :       got_rtk_counter_++;
+    1661          20 :       rtk_avg = rtk_init_z_avg_ / got_rtk_counter_;
+    1662          20 :       ROS_INFO("[%s]: RTK ASL altitude sample #%d: %.2f; avg: %.2f", getPrintName().c_str(), got_rtk_counter_, rtk_z, rtk_avg);
+    1663          20 :       return;
+    1664             : 
+    1665             :     } else {
+    1666             : 
+    1667           2 :       rtk_init_z_avg_     = rtk_avg;
+    1668           2 :       got_avg_init_rtk_z_ = true;
+    1669           2 :       ROS_INFO("[%s]: RTK ASL altitude avg: %f", getPrintName().c_str(), rtk_avg);
+    1670             :     }
+    1671             :   }
+    1672             : }
+    1673             : /*//}*/
+    1674             : 
+    1675             : /*//{ checkMsgDelay() */
+    1676             : template <int n_measurements>
+    1677             : void Correction<n_measurements>::checkMsgDelay(const ros::Time& msg_time) {
+    1678             : 
+    1679             :   const double delay = (ros::Time::now() - msg_time).toSec();
+    1680             :   if (delay > msg_delay_warn_limit_) {
+    1681             :     if (delay > msg_delay_limit_) {
+    1682             :       ROS_ERROR_THROTTLE(1.0, "[%s]: message too delayed (%.4f s)", getPrintName().c_str(), delay);
+    1683             :       is_delay_ok_ = false;
+    1684             :     } else {
+    1685             :       ROS_WARN_THROTTLE(5.0, "[%s]: message delayed (%.4f s)", getPrintName().c_str(), delay);
+    1686             :       is_delay_ok_ = true;
+    1687             :     }
+    1688             :   } else {
+    1689             :     is_delay_ok_ = true;
+    1690             :   }
+    1691             :   publishDelay(delay);
+    1692             : }
+    1693             : /*//}*/
+    1694             : 
+    1695             : /*//{ isTimestampOk() */
+    1696             : template <int n_measurements>
+    1697             : bool Correction<n_measurements>::isTimestampOk() {
+    1698             : 
+    1699             :   if (first_timestamp_) {
+    1700             :     return true;
+    1701             :   }
+    1702             : 
+    1703             :   ros::Time msg_time, prev_msg_time;
+    1704             :   {
+    1705             :     std::scoped_lock lock(mtx_msg_time_);
+    1706             :     msg_time      = msg_time_;
+    1707             :     prev_msg_time = prev_msg_time_;
+    1708             :   }
+    1709             :   const double delta = msg_time.toSec() - prev_msg_time.toSec();
+    1710             : 
+    1711             :   if (msg_time.toSec() <= 0.0) {
+    1712             :     ROS_ERROR_THROTTLE(1.0, "[%s]: current timestamp non-positive: %f", getPrintName().c_str(), msg_time.toSec());
+    1713             :     return false;
+    1714             :   }
+    1715             : 
+    1716             :   if (delta <= 0.0) {
+    1717             :     ROS_WARN_THROTTLE(1.0, "[%s]: time delta non-positive: %f", getPrintName().c_str(), delta);
+    1718             :     return true;
+    1719             :   }
+    1720             : 
+    1721             :   if (delta < 0.001) {
+    1722             :     ROS_WARN_THROTTLE(1.0, "[%s]: time delta too small: %f", getPrintName().c_str(), delta);
+    1723             :     return true;
+    1724             :   }
+    1725             : 
+    1726             :   if (delta > time_since_last_msg_limit_) {
+    1727             :     ROS_ERROR_THROTTLE(1.0, "[%s]: time since last msg too long %f > %f", getPrintName().c_str(), delta, time_since_last_msg_limit_);
+    1728             :     return false;
+    1729             :   }
+    1730             : 
+    1731             :   return true;
+    1732             : }  // namespace mrs_uav_state_estimators
+    1733             : /*//}*/
+    1734             : 
+    1735             : /*//{ isMsgComing() */
+    1736             : template <int n_measurements>
+    1737      674414 : bool Correction<n_measurements>::isMsgComing() {
+    1738             : 
+    1739      674414 :   if (first_timestamp_) {
+    1740           0 :     return true;
+    1741             :   }
+    1742             : 
+    1743      674331 :   const ros::Time msg_time = mrs_lib::get_mutexed(mtx_msg_time_, msg_time_);
+    1744      674457 :   const double    delta    = ros::Time::now().toSec() - msg_time.toSec();
+    1745             : 
+    1746      674471 :   if (msg_time.toSec() <= 0.0) {
+    1747           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: current timestamp non-positive: %f", getPrintName().c_str(), msg_time.toSec());
+    1748           0 :     return false;
+    1749             :   }
+    1750             : 
+    1751      674502 :   if (delta > time_since_last_msg_limit_) {
+    1752           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: time since last msg too long %f > %f", getPrintName().c_str(), delta, time_since_last_msg_limit_);
+    1753           0 :     return false;
+    1754             :   }
+    1755             : 
+    1756      674502 :   return true;
+    1757             : }  // namespace mrs_uav_state_estimators
+    1758             : /*//}*/
+    1759             : 
+    1760             : /*//{ createProcessorFromName() */
+    1761             : template <int n_measurements>
+    1762         332 : std::shared_ptr<Processor<n_measurements>> Correction<n_measurements>::createProcessorFromName(const std::string& name, ros::NodeHandle& nh) {
+    1763             : 
+    1764         332 :   if (name == "median_filter") {
+    1765          77 :     return std::make_shared<ProcMedianFilter<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1766         255 :   } else if (name == "saturate") {
+    1767          83 :     return std::make_shared<ProcSaturate<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_, state_id_, fun_get_state_);
+    1768         172 :   } else if (name == "excessive_tilt") {
+    1769          77 :     return std::make_shared<ProcExcessiveTilt<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1770          95 :   } else if (name == "tf_to_world") {
+    1771          95 :     return std::make_shared<ProcTfToWorld<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1772             :   } else {
+    1773           0 :     ROS_ERROR("[%s]: requested invalid processor %s", getPrintName().c_str(), name.c_str());
+    1774           0 :     ros::shutdown();
+    1775             :   }
+    1776           0 :   return std::shared_ptr<Processor<n_measurements>>(nullptr);
+    1777             : }
+    1778             : /*//}*/
+    1779             : 
+    1780             : /*//{ process() */
+    1781             : template <int n_measurements>
+    1782      673393 : bool Correction<n_measurements>::process(Correction<n_measurements>::measurement_t& measurement) {
+    1783             : 
+    1784      673393 :   bool ok_flag   = true;
+    1785      673393 :   bool fuse_flag = true;
+    1786             : 
+    1787     1290311 :   for (auto proc_name :
+    1788             :        processor_names_) {  // need to access the processors in the specific order from the config (e.g. median filter should go before saturation etc.)
+    1789             :     /* bool is_ok, should_fuse; */
+    1790      617145 :     auto [is_ok, should_fuse] = processors_[proc_name]->process(measurement);
+    1791      616868 :     ok_flag &= is_ok;
+    1792      616868 :     fuse_flag &= should_fuse;
+    1793             :   }
+    1794      673170 :   if (fuse_flag) {
+    1795      658229 :     if (!ok_flag) {
+    1796         306 :       setR(default_R_ * R_coeff_);
+    1797         306 :       ROS_INFO_THROTTLE(1.0, "[%s]: set R to %.4f", getPrintName().c_str(), default_R_ * R_coeff_);
+    1798         306 :       return true;
+    1799             :     } else {
+    1800      657923 :       setR(default_R_);
+    1801      657933 :       return true;
+    1802             :     }
+    1803             :   }
+    1804       14941 :   return false;
+    1805             : }
+    1806             : /*//}*/
+    1807             : 
+    1808             : /*//{ resetProcessors() */
+    1809             : template <int n_measurements>
+    1810           0 : void Correction<n_measurements>::resetProcessors() {
+    1811             : 
+    1812           0 :   for (auto proc_name :
+    1813             :        processor_names_) {  // need to access the processors in the specific order from the config (e.g. median filter should go before saturation etc.)
+    1814             :     /* bool is_ok, should_fuse; */
+    1815           0 :     processors_[proc_name]->reset();
+    1816             :   }
+    1817           0 : }
+    1818             : /*//}*/
+    1819             : 
+    1820             : /*//{ publishCorrection() */
+    1821             : template <int n_measurements>
+    1822     1331618 : void Correction<n_measurements>::publishCorrection(const MeasurementStamped&                                 measurement_stamped,
+    1823             :                                                    mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr) {
+    1824             : 
+    1825     1331618 :   if (!ch_->debug_topics.correction) {
+    1826           0 :     return;
+    1827             :   }
+    1828             : 
+    1829     2663004 :   mrs_msgs::EstimatorCorrection msg;
+    1830     1330949 :   msg.header.stamp    = measurement_stamped.stamp;
+    1831     1330949 :   msg.header.frame_id = ns_frame_id_;
+    1832     1331514 :   msg.name            = name_;
+    1833     1331549 :   msg.estimator_name  = est_name_;
+    1834     1331572 :   msg.state_id        = state_id_;
+    1835     1331572 :   msg.covariance.resize(n_measurements * n_measurements);
+    1836     3066577 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+    1837     1735401 :     msg.state.push_back(measurement_stamped.value(i));
+    1838     1735885 :     msg.covariance[n_measurements * i + i] = getR();
+    1839             :   }
+    1840             : 
+    1841     1330883 :   ph_corr.publish(msg);
+    1842             : }
+    1843             : /*//}*/
+    1844             : 
+    1845             : /*//{ publishDelay() */
+    1846             : template <int n_measurements>
+    1847             : void Correction<n_measurements>::publishDelay(const double delay) {
+    1848             : 
+    1849             :   if (!ch_->debug_topics.corr_delay) {
+    1850             :     return;
+    1851             :   }
+    1852             : 
+    1853             :   mrs_msgs::Float64Stamped msg;
+    1854             :   msg.header.stamp    = ros::Time::now();
+    1855             :   msg.header.frame_id = ns_frame_id_;
+    1856             :   msg.value           = delay;
+    1857             : 
+    1858             :   ph_delay_.publish(msg);
+    1859             : }
+    1860             : /*//}*/
+    1861             : 
+    1862             : }  // namespace mrs_uav_state_estimators
+    1863             : 
+    1864             : #endif  // ESTIMATORS_CORRECTION_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html new file mode 100644 index 0000000000..c1ed2581c2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html @@ -0,0 +1,486 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..395ca77dfa6fb6d34b0cf0eb46b7381e5920351b GIT binary patch literal 5376 zcmV+b760mqP)|Te0YV4?lHTt3dwaj%^yUC# zYqtZM+l;1h*tR3fF^0B3AkBFW$jXPnNcUa>S}=0mR$(+ZjIpX~Mnp%`D4<)*%vJ&m zj62LNfN>9O7-P}QfOj(>0@eWQSrjqW&=~tx04p%wjexsyU5#r+*R7?)z0?(TZCeZN zU7ICl9w#eYYaOte=?L7mI}Grb$kEt)P1jb8Hn>B&HnMGnYmHil)a7DfTPJAHZ1bvqf@0)ooJv7)?zMFe=M^ zF)MtuAOW^Uz}xN^y^Z8qx3oAc&7ND$e6EnP=K(FcM|W*Aj}-_joH#f#Z+UK|Ar}u2 zQkwy4-}efLF>xK$P)*aA2D43oCLAn{CEyM;B2u?vq!t1iEACg|8ok+gK$)N>tP~1h zvK%z)0cIZa z`3L9co}BfjAYaGGHy@I~9Vk{&4h;p!SZfL+Av>_K)w<4@Z4}oP2ydg0 z<~yVrC;(f<`guTrcpG&9qs0<P0Bq3PJRpYKv??^@>ne=5j4)>Yzz#GT0aZI?Kle={)26v8eq~lJ$le-(BP4?N zg#fK_3g^nRu6c}nNCSfaCHBInDvjo58|GqiF@e>pEl7qAGXq6Ibks!oubhSSL3IJ^)rvDEiN?+w&2%YT^1n#G9G)=Y&~wV^=6Gim#u8P`cHjw4Rs z%oygQc6~BQ#@-(Ru?ec5fUD z%Lh&5vLHRx#mHl{8OV%Aj>1HvNaUZ;_AN+FRr;y zrsKz&9c(Wiay%YkVCVl2kYlwz@Z8erl-zefUo z`%yGX^O>VD5Q^q`UDrpP5NR4vQF#v~?aPk_IdRlh%KZjnDi3al1O*XBCcQgGZ38cp zc>HhEQ`E@=_FR>6BwYJgYl!8j+6uT$;nvGwDF7}J(7K;7)96M8mA2Bksz?+au z2r^H)%nRJLVj0PUF0(nApwf<>n{$IMXq?#rt&bKC{8q*R8kMKJNpvAV`%s5L)d6d= zDOA8N)Ag{GQW>&~#}J~dY`D<yJ|2GBj@?H{LpT*}z_IIU z*GHl{fCBsgYo^-!IwuaThLqW_t4pb~X1ewqnM4L@dy*MD*T!U%8DmXW$|jy?vONO6 zWP8-Hsj)rA_H=B=Zzdy8FoUEc0giNR=KUp1<5Jk%TDI8MqptLAJ6q1lzP}u%wM^&E z%=nQhVvm$kEK9Vi6i;RhDZt4ZYu;ILpSuUn&8;R=Ve9&d3;>VvGYtSH<}v`EVgPdY z@G<~0pt&7ZPm$MBl%xLyn-7V352$-_b~*09S*S^!8@;^_c)5bZmuvV4Hn7X@lYe{Z z`eBIL&44>ckPtJPcuu4db_~kbi>~3`1*8H?8kv+>hOwrdWtz>+%Q36Fph^avc+@)W zpcI9~DL_-<_W^3w?OsJe#nT2BcyshgF>!XbE<7Q!g{`#7QjA$U&*N&YAG`FMEF8@{D_1um4 z(YX(LWUkx?4`^=AMA>W)v}KDK8e%O_Kg1(>&jV5#XC7E`2R@AVe9X&-BF9B)eLoPl zu;6NLW^v8k3vy)oM>;&!`@hMgjB;(`SBy;HS(7+<6#-R^w5A3LV<-SWq=HS%gD;<+ zRlz2W2L|*nk%LA7T0Ap++zkZe%Cbay7E*V*bmYPB@(h6o)b;0P^a>M3M=D<_Ug5+U zD_C#Fo-1xQX&Z1n6hz%9#r^oI&ll~PEz2YPrNYNe7My3 zmG>Zx3|2ccX4N^|H1oc5g_;qPA#|-q&0^_2s)scG^__4i4RFYiQ@%uZu`y!*RxrUdOF?@A(#!!eh&jK-6o4XE%id~j7bDc z?l4v3I7UF#qy;ro)dB%>`_>Fl9_?FXK(V@*#wg2IIHf-Ms{tp}wE?4UpPgn{91uvN zG`sLv3zKFApfbB|My5U2l3j%Dh*_ww#K`spq5Hc=CTO#P?J;1qEZJ2|pCiSk=TP)1 zU*laYag5#j&zHDs<)XwXuUr(MymGz9UtPI=X+gTy88<7gb!O@L+DdVAD*%eo5eHbv zl6$1j6Z8#)S?sleA$1#w=@q3#(3t1#2$BguobG#zXYbH4fCl@RBa~8X&wZ_#M>r|b zu#(1LQ>J)F9;kJ-*p#oZxUTC42AqU6?3~LrWof|!qm&3%di)G`uPg*K+_3Vd(N z_m3A*!SDF_BJh5~U>Put`V}WtG+Lg6vuh=3maOcrRA4Qq`0kxa) zqK4c5Cd1)eX9yA96Vel+}` z#Ldk6LZzHJwxe$5JSY5MJ94_Rq&kyc4rnd~wlZ^g_~8%A=fi1`Rca=V4-O2mS7U`o zjhuB;Y^ICcgAU0h!J9S5Ra`VrWTu=5+poO^lZiN~Pyj9f)B~0oK=sLzb%4c5+CDs# zvWn~SL~cG|9;+Yb&78-rrLopNDo2cO;CzT^DeVt<=I-POufp&^t_i7^G#a}8CS$Lq z;I+V(G-M=xdi_@i8`8yh-(E-h+6Z_RoI2h3?4 z`%KQrj^Rz&l<>i_(XYs+nB|l9%-S>KjR_j?;P&NOwVuNp0OxmSATdM|5J)8- z=$Y{vM(cyytt~4`k-GMuixh%Hj*nhMIhKIdM2?-AT>V%Y6JRe@&}4v8HBP4gAO({H z=7*Fxi~`C*1azPQ8~|DcZiMS@lExrFbMpt4g4x~WDl0xfkS|EKb*AnPwo%Xe9&!g; zpwX|pd^n7d#b43rw^k$5XQI*C^}UJCdb_Ac6aC1LZKzvS)0HYR`v5S#7~u=g;TknF zg_Arf=j+t)kB+sl_%dVQ*?12A*bgelzK zrMO3zQ^a)*a$z!!M*((&ws;~}0l1^Z#k;pIhhi}r8-L?&R$5YDF(lSgLqOgGhk(c3 zkK21%hvod4JAxK#kbd&o+Z_+uKq>M{bcCg_HlsSOJ-sg&9}gJRTisMFfJ)cvX{7m| z0nr<=Yyh=>g#nNY5FlWOFNN3$_!pLb{Tci9P3qNdi>4^5n@Y09p~2<-6ye@XX#3{_W8CQtKW%g;Pyzt95?fExN+sB2f? z>Yc;h)pLd+4j4_$nA&asP#j>!Piav+Nj4l?w0H)r_q`L3HP5ws;ng?nukSENf|`$Y z&;QkN5%SyjCZbNSAgX2gKo^6uC=sMY55Sg+KYR&D6i5PW)F{4vDqz_KFTagfP^RQ5(ey zh0LuPGFetM5Mwfn%FG4*ph~tTNUilG+>PnTqmEQGhBX3ClwcDvBZwKj>w}nSEi;*B zrop!ULzn?dME6ptSy;0gm-lW$rtCyDvx>1n7K)#ul5JQ^;vZe5U&IfNx?#YOz&_@N zfsiaC!fI(Z9{`tA>NwnkcU7yz(@TLG`ugB-A}7RXKt%J5vK=41Lb5Ym*zSn;&n;Yn zP4PU}iOehmuMgqSC*=u^1U%3HXl9_o08rDve5+E!(;cY1hJ!p8*;3xsC8^>GQx2tq zF%4LOkwdD|2=4jFF!D#gMU3&2HF4Z%9!6jw;;aLJdrhu%YKUr}{OSB-T85KA8yC%@ zRTi-LQf>>NQV3kPAPr$PRI<%571f0U-##$5JiNf^?=2p7X%r(BE+R;miBN6ub4Cu2 z1{xoaGzX;+vNeC*DK-ao-Rn}sVa1Sv&;Y$*80K&Ec;y*lcvfG#Zl)^`vaVI7Fn6u@ z+f|*Z@Ip~Ben#45r?N)Y-ewikHHEvOc2zIj9aC@NjMNnBUgLu;Ze{~eR&#S*TNfnr zI`h(fOb57TtamFnG584&$=GM#w=+{dy@)m&C*Ko?)(NG8?NR1>S-`j$aZX(Nwpc+6 zFm{TuPZXH|(a?mk!Ut`ZTvgu><-&?q0qR5!DWC#ym{Ak;eutiwQWm6-F!ss`RkHzL zGiTnoR%Xo>lsQH|A!ZcNamQG_oOHktwP#6Af@>u(pMY3i$(8%|21iYTJ28D_j%b?O ei~wmG1NRU4vl6or)-AXI0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-04-25 05:42:57Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgGeneric::HdgGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)0
mrs_uav_state_estimators::HdgGeneric::~HdgGeneric()0
mrs_uav_state_estimators::HdgGeneric::~HdgGeneric().20
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html new file mode 100644 index 0000000000..4b5dc04c44 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-04-25 05:42:57Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgGeneric::HdgGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)0
mrs_uav_state_estimators::HdgGeneric::~HdgGeneric()0
mrs_uav_state_estimators::HdgGeneric::~HdgGeneric().20
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.frameset.html new file mode 100644 index 0000000000..d9b5d32060 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html new file mode 100644 index 0000000000..ce11a8ac65 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html @@ -0,0 +1,243 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-04-25 05:42:57Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_HEADING_HDG_GENERIC_H
+       2             : #define ESTIMATORS_HEADING_HDG_GENERIC_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <nav_msgs/Odometry.h>
+       9             : 
+      10             : #include <mrs_lib/lkf.h>
+      11             : #include <mrs_lib/repredictor.h>
+      12             : #include <mrs_lib/profiler.h>
+      13             : #include <mrs_lib/param_loader.h>
+      14             : #include <mrs_lib/subscribe_handler.h>
+      15             : #include <mrs_lib/geometry/cyclic.h>
+      16             : 
+      17             : #include <mrs_uav_state_estimators/estimators/heading/heading_estimator.h>
+      18             : #include <mrs_uav_state_estimators/estimators/correction.h>
+      19             : 
+      20             : #include <mrs_uav_state_estimators/HeadingEstimatorConfig.h>
+      21             : 
+      22             : //}
+      23             : 
+      24             : namespace mrs_uav_state_estimators
+      25             : {
+      26             : 
+      27             : namespace hdg_generic
+      28             : {
+      29             : 
+      30             : const int n_states       = 2;
+      31             : const int n_inputs       = 1;
+      32             : const int n_measurements = 1;
+      33             : 
+      34             : }  // namespace hdg_generic
+      35             : 
+      36             : class HdgGeneric : public HeadingEstimator<hdg_generic::n_states> {
+      37             : 
+      38             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      39             : 
+      40             :   typedef mrs_lib::DynamicReconfigureMgr<HeadingEstimatorConfig> drmgr_t;
+      41             : 
+      42             :   using lkf_t         = mrs_lib::LKF<hdg_generic::n_states, hdg_generic::n_inputs, hdg_generic::n_measurements>;
+      43             :   using varstep_lkf_t = mrs_lib::varstepLKF<hdg_generic::n_states, hdg_generic::n_inputs, hdg_generic::n_measurements>;
+      44             :   using A_t           = lkf_t::A_t;
+      45             :   using B_t           = lkf_t::B_t;
+      46             :   using H_t           = lkf_t::H_t;
+      47             :   using Q_t           = lkf_t::Q_t;
+      48             :   using x_t           = lkf_t::x_t;
+      49             :   using P_t           = lkf_t::P_t;
+      50             :   using u_t           = lkf_t::u_t;
+      51             :   using z_t           = lkf_t::z_t;
+      52             :   using R_t           = lkf_t::R_t;
+      53             :   using statecov_t    = lkf_t::statecov_t;
+      54             : 
+      55             :   typedef mrs_lib::Repredictor<varstep_lkf_t> rep_lkf_t;
+      56             : 
+      57             :   using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;
+      58             : 
+      59             : private:
+      60             :   std::string parent_state_est_name_;
+      61             : 
+      62             :   double                                      dt_;
+      63             :   double                                      input_coeff_;
+      64             :   double                                      default_input_coeff_;
+      65             :   A_t                                         A_;
+      66             :   B_t                                         B_;
+      67             :   H_t                                         H_;
+      68             :   Q_t                                         Q_;
+      69             :   std::shared_ptr<lkf_t>                      lkf_;
+      70             :   std::unique_ptr<rep_lkf_t>                  lkf_rep_;
+      71             :   std::vector<std::shared_ptr<varstep_lkf_t>> models_;
+      72             :   mutable std::mutex                          mutex_lkf_;
+      73             :   statecov_t                                  sc_;
+      74             :   mutable std::mutex                          mutex_sc_;
+      75             : 
+      76             :   std::unique_ptr<drmgr_t> drmgr_;
+      77             :   void                     callbackReconfigure(HeadingEstimatorConfig &config, [[maybe_unused]] uint32_t level);
+      78             : 
+      79             :   z_t                innovation_;
+      80             :   mutable std::mutex mtx_innovation_;
+      81             : 
+      82             :   bool is_repredictor_enabled_;
+      83             :   int  rep_buffer_size_ = 200;
+      84             : 
+      85             :   const bool is_core_plugin_;
+      86             : 
+      87             :   std::vector<std::string>                                              correction_names_;
+      88             :   std::vector<std::shared_ptr<Correction<hdg_generic::n_measurements>>> corrections_;
+      89             : 
+      90             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput> sh_control_input_;
+      91             :   void                                                timeoutCallback(const std::string &topic, const ros::Time &last_msg);
+      92             :   std::atomic<bool>                                   is_input_ready_ = false;
+      93             : 
+      94             :   ros::Timer timer_update_;
+      95             :   void       timerUpdate(const ros::TimerEvent &event);
+      96             : 
+      97             :   ros::Timer timer_check_health_;
+      98             :   void       timerCheckHealth(const ros::TimerEvent &event);
+      99             : 
+     100             :   void doCorrection(const Correction<hdg_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id);
+     101             :   void doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp);
+     102             : 
+     103             :   bool isConverged();
+     104             : 
+     105             :   Q_t                getQ();
+     106             :   mutable std::mutex mtx_Q_;
+     107             : 
+     108             :   mutable std::mutex mutex_last_valid_hdg_;
+     109             :   double             last_valid_hdg_;
+     110             : 
+     111             : public:
+     112           0 :   HdgGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+     113           0 :       : HeadingEstimator<hdg_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+     114           0 :   }
+     115             : 
+     116           0 :   ~HdgGeneric(void) {
+     117           0 :   }
+     118             : 
+     119             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     120             :   bool start(void) override;
+     121             :   bool pause(void) override;
+     122             :   bool reset(void) override;
+     123             : 
+     124             :   double getState(const int &state_idx_in) const override;
+     125             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     126             : 
+     127             :   void setState(const double &state_in, const int &state_idx_in) override;
+     128             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     129             : 
+     130             :   states_t getStates(void) const override;
+     131             :   void     setStates(const states_t &states_in) override;
+     132             : 
+     133             :   double getCovariance(const int &state_idx_in) const override;
+     134             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     135             : 
+     136             :   covariance_t getCovarianceMatrix(void) const override;
+     137             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     138             : 
+     139             :   double getInnovation(const int &state_idx) const override;
+     140             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     141             : 
+     142             :   double getLastValidHdg() const override;
+     143             : 
+     144             :   void setDt(const double &dt);
+     145             :   void setInputCoeff(const double &input_coeff);
+     146             : 
+     147             :   void generateRepredictorModels(const double input_coeff);
+     148             : 
+     149             :   void generateA();
+     150             :   void generateB();
+     151             : 
+     152             : 
+     153             :   std::string getNamespacedName() const;
+     154             : 
+     155             :   std::string getPrintName() const;
+     156             : };
+     157             : }  // namespace mrs_uav_state_estimators
+     158             : 
+     159             : #endif  // ESTIMATORS_HEADING_HDG_GENERIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html new file mode 100644 index 0000000000..f648f9d8d0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html @@ -0,0 +1,60 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c826730ad70614286d57a68cb29d1eed954df391 GIT binary patch literal 663 zcmV;I0%-k-P)4zK6(s~1FktrePxJa#)*WXO@}bs+5PYc2AyWg@7ASK1 zmo$>`V`ShNVzi=_j5E@Ga0&E!#xAr5cGX+pFeHTm>DC^BgH2eKUSeGGUW|YR z7&{Nn{v{A*73@1M0FWYBF}}EpfuKx5)^W|@6!sV!tj=T%G@_9|!6&8Y7|lqhO`wNH z;HZh3g-;(@)7!jR#8}J%xfo%I*N{%(K31_-x+M}G^;wH+^4D%7$DuvpqLY;~ z-0$njF*!iuxSFPdXk6j0=qfLdi_%n+A*+D#!T7|JPxr`>pUGyW&0YqINimidlfp*2 zo;|&|SkS7s)fC%t+fVx3t7C + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgPassthrough::HdgPassthrough(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)99
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()99
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html new file mode 100644 index 0000000000..1ffb68c0fb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgPassthrough::HdgPassthrough(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)99
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()99
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.frameset.html new file mode 100644 index 0000000000..b8c92e1d37 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html new file mode 100644 index 0000000000..f3ff145f11 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html @@ -0,0 +1,191 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_HEADING_HDG_PASSTHROUGH_H
+       2             : #define ESTIMATORS_HEADING_HDG_PASSTHROUGH_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <nav_msgs/Odometry.h>
+       9             : 
+      10             : #include <mrs_lib/lkf.h>
+      11             : #include <mrs_lib/profiler.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : #include <mrs_lib/subscribe_handler.h>
+      14             : #include <mrs_lib/geometry/cyclic.h>
+      15             : 
+      16             : #include <mrs_uav_state_estimators/estimators/heading/heading_estimator.h>
+      17             : 
+      18             : //}
+      19             : 
+      20             : namespace mrs_uav_state_estimators
+      21             : {
+      22             : 
+      23             : namespace hdg_passthrough
+      24             : {
+      25             : const int n_states = 2;
+      26             : }  // namespace hdg_passthrough
+      27             : 
+      28             : using namespace mrs_uav_managers::estimation_manager;
+      29             : 
+      30             : class HdgPassthrough : public HeadingEstimator<hdg_passthrough::n_states> {
+      31             : 
+      32             :   using CommonHandlers_t = mrs_uav_managers::estimation_manager::CommonHandlers_t;
+      33             : 
+      34             : private:
+      35             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      36             : 
+      37             :   std::string parent_state_est_name_;
+      38             : 
+      39             :   states_t           hdg_state_;
+      40             :   mutable std::mutex mtx_hdg_state_;
+      41             :   states_t           prev_hdg_state_;
+      42             :   mutable std::mutex mtx_prev_hdg_state_;
+      43             : 
+      44             :   covariance_t       hdg_covariance_;
+      45             :   mutable std::mutex mtx_hdg_covariance_;
+      46             : 
+      47             :   states_t           innovation_;
+      48             :   mutable std::mutex mtx_innovation_;
+      49             : 
+      50             :   const bool is_core_plugin_;
+      51             : 
+      52             :   std::string                                                 orient_topic_;
+      53             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_;
+      54             :   void                                                        callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg);
+      55             :   std::atomic<bool>                                           is_orient_ready_ = false;
+      56             : 
+      57             :   std::string                                              ang_vel_topic_;
+      58             :   mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped> sh_ang_vel_;
+      59             :   void                                                     callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg);
+      60             :   std::atomic<bool>                                        is_ang_vel_ready_ = false;
+      61             : 
+      62             :   ros::Timer timer_update_;
+      63             :   void       timerUpdate(const ros::TimerEvent &event);
+      64             : 
+      65             :   ros::Timer timer_check_health_;
+      66             :   void       timerCheckHealth(const ros::TimerEvent &event);
+      67             : 
+      68             : public:
+      69          99 :   HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+      70          99 :       : HeadingEstimator<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+      71          99 :   }
+      72             : 
+      73         198 :   ~HdgPassthrough(void) {
+      74         198 :   }
+      75             : 
+      76             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+      77             :   bool start(void) override;
+      78             :   bool pause(void) override;
+      79             :   bool reset(void) override;
+      80             : 
+      81             :   double getState(const int &state_idx_in) const override;
+      82             :   double getState(const int &state_id_in, const int &axis_in) const override;
+      83             : 
+      84             :   void setState(const double &state_in, const int &state_idx_in) override;
+      85             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+      86             : 
+      87             :   states_t getStates(void) const override;
+      88             :   void     setStates(const states_t &states_in) override;
+      89             : 
+      90             :   double getCovariance(const int &state_idx_in) const override;
+      91             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+      92             : 
+      93             :   covariance_t getCovarianceMatrix(void) const override;
+      94             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+      95             : 
+      96             :   double getInnovation(const int &state_idx) const override;
+      97             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+      98             : 
+      99             :   double getLastValidHdg() const override;
+     100             : 
+     101             :   std::string getNamespacedName() const;
+     102             : 
+     103             :   std::string getPrintName() const;
+     104             : };
+     105             : }  // namespace mrs_uav_state_estimators
+     106             : 
+     107             : #endif  // ESTIMATORS_HEADING_HDG_PASSTHROUGH_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.overview.html new file mode 100644 index 0000000000..3a0362ef13 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.overview.html @@ -0,0 +1,47 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..59c70c2f6a8a81135bd8dca6f6b93ea23cdae157 GIT binary patch literal 506 zcmVD=e3=CiAy9U z<2PVAFPU){EoYpNMstpRXbtRU&;m`?%w{?<$Eftk`yA1d&vxZPT4gs&-<5UsI_S?J zpjq86U3cXgc_sr8i7)Osaz+En#GE0tqLVbH#FzVs)9o2Wax+>^NqDrTRgC={8^&cj zl-U1B#|8}88tDx~h#T3S90$A0<5nmnHaAlv0*87xQXh`=FKjYX%w^3>F{+xG;tZjg zAs|ef0mD!iWk9n7Mvob#Rhm+o?Ksra^UAiQ!%6W;*CT$;bdC2egm9dlIM|m9M=*u6 z!7dpF07*qoM6N<$g7Y-qe*gdg literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html new file mode 100644 index 0000000000..99a9580596 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HeadingEstimator<2>::HeadingEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html new file mode 100644 index 0000000000..a4d009b65b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HeadingEstimator<2>::HeadingEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.frameset.html new file mode 100644 index 0000000000..efa10b4b9b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html new file mode 100644 index 0000000000..0463f96c50 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-25 05:42:57Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_HEADING_HEADING_ESTIMATOR_H
+       2             : #define ESTIMATORS_HEADING_HEADING_ESTIMATOR_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <mrs_uav_state_estimators/estimators/partial_estimator.h>
+       7             : 
+       8             : //}
+       9             : 
+      10             : namespace mrs_uav_state_estimators
+      11             : {
+      12             : 
+      13             : namespace heading
+      14             : {
+      15             : const char type[] = "HEADING";
+      16             : }
+      17             : 
+      18             : template <int n_states>
+      19             : class HeadingEstimator : public PartialEstimator<n_states, 1> {
+      20             : 
+      21             : protected:
+      22          99 :   HeadingEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(heading::type, name, frame_id) {
+      23          99 :   }
+      24             : 
+      25             :   mutable std::mutex mutex_last_valid_hdg_;
+      26             :   double             last_valid_hdg_;
+      27             : 
+      28             : private:
+      29             :   static const int _n_axes_   = 1;
+      30             :   static const int _n_states_ = n_states;
+      31             :   static const int _n_inputs_;
+      32             :   static const int _n_measurements_;
+      33             : 
+      34             : public:
+      35             :   virtual double getLastValidHdg() const = 0;
+      36             : };
+      37             : 
+      38             : }  // namespace mrs_uav_state_estimators
+      39             : 
+      40             : #endif  // ESTIMATORS_HEADING_HEADING_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..4cc90f7128 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.overview.html @@ -0,0 +1,30 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3fa1a030f3bdb9d4e5e4354fd133a23a3d84dca9 GIT binary patch literal 293 zcmeAS@N?(olHy`uVBq!ia0vp^0YI$5!VDy5`z06wDTx4|5ZC|z|E~gq#sJy z_!d=lu%pyiZdT`(8i5lD0#}Z_-Sjn9u0vVe{n2K}X{;w7Y_kv6QkQ>GW1X*;rZGj% iUS^U*{*!sPE144yXqeA-@c#>RK7*&LpUXO@geCwTJ$YaN literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html new file mode 100644 index 0000000000..9db7b70929 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-25 05:42:57Functions:4757.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.h +
0.0%
+ 		0.0 %0 / 50.0 %0 / 3
heading_estimator.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
hdg_passthrough.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
<unnamed>100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html new file mode 100644 index 0000000000..9c802c0db8 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-25 05:42:57Functions:4757.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.h +
0.0%
+ 		0.0 %0 / 50.0 %0 / 3
heading_estimator.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
hdg_passthrough.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
<unnamed>100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html new file mode 100644 index 0000000000..56fab601a1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-25 05:42:57Functions:4757.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.h +
0.0%
+ 		0.0 %0 / 50.0 %0 / 3
hdg_passthrough.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
<unnamed>100.0 %5 / 5100.0 %3 / 3
heading_estimator.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html new file mode 100644 index 0000000000..881b00fbdf --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-25 05:42:57Functions:4757.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.h +
0.0%
+ 		0.0 %0 / 50.0 %0 / 3
heading_estimator.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
hdg_passthrough.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html new file mode 100644 index 0000000000..b4f673c942 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-25 05:42:57Functions:4757.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.h +
0.0%
+ 		0.0 %0 / 50.0 %0 / 3
heading_estimator.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
hdg_passthrough.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html new file mode 100644 index 0000000000..2ce030d506 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-25 05:42:57Functions:4757.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.h +
0.0%
+ 		0.0 %0 / 50.0 %0 / 3
hdg_passthrough.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
heading_estimator.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-f.html new file mode 100644 index 0000000000..30cabcb058 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:40776353.3 %
Date:2024-04-25 05:42:57Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.3%51.3%
+ 		51.3 %373 / 72766.2 %47 / 71
<unnamed>51.3 %373 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html new file mode 100644 index 0000000000..a7b337d199 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:40776353.3 %
Date:2024-04-25 05:42:57Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.3%51.3%
+ 		51.3 %373 / 72766.2 %47 / 71
<unnamed>51.3 %373 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html new file mode 100644 index 0000000000..efc164ea3b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:40776353.3 %
Date:2024-04-25 05:42:57Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.3%51.3%
+ 		51.3 %373 / 72766.2 %47 / 71
<unnamed>51.3 %373 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html new file mode 100644 index 0000000000..b431127560 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:40776353.3 %
Date:2024-04-25 05:42:57Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.3%51.3%
+ 		51.3 %373 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html new file mode 100644 index 0000000000..b0bbedcae6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:40776353.3 %
Date:2024-04-25 05:42:57Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.3%51.3%
+ 		51.3 %373 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html new file mode 100644 index 0000000000..0fe215a34d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:40776353.3 %
Date:2024-04-25 05:42:57Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.3%51.3%
+ 		51.3 %373 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html new file mode 100644 index 0000000000..dc58b4989a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
lat_generic.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
<unnamed>100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html new file mode 100644 index 0000000000..01ae45eac1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
lat_generic.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
<unnamed>100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html new file mode 100644 index 0000000000..f59bfacb82 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
<unnamed>100.0 %6 / 6100.0 %3 / 3
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html new file mode 100644 index 0000000000..67731f684a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
lat_generic.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html new file mode 100644 index 0000000000..035e9df6f4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
lat_generic.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html new file mode 100644 index 0000000000..f45ff73a8d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html new file mode 100644 index 0000000000..69699a19f7 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::LatGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool, std::function<std::optional<double> ()>)99
mrs_uav_state_estimators::LatGeneric::~LatGeneric()99
mrs_uav_state_estimators::LatGeneric::~LatGeneric().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html new file mode 100644 index 0000000000..4178cf64bb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::LatGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool, std::function<std::optional<double> ()>)99
mrs_uav_state_estimators::LatGeneric::~LatGeneric()99
mrs_uav_state_estimators::LatGeneric::~LatGeneric().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.frameset.html new file mode 100644 index 0000000000..7be2806e41 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html new file mode 100644 index 0000000000..7795341348 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html @@ -0,0 +1,250 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_LATERAL_LAT_GENERIC_H
+       2             : #define ESTIMATORS_LATERAL_LAT_GENERIC_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <nav_msgs/Odometry.h>
+       9             : 
+      10             : #include <sensor_msgs/Imu.h>
+      11             : 
+      12             : #include <mrs_lib/lkf.h>
+      13             : #include <mrs_lib/repredictor.h>
+      14             : #include <mrs_lib/profiler.h>
+      15             : #include <mrs_lib/param_loader.h>
+      16             : #include <mrs_lib/subscribe_handler.h>
+      17             : 
+      18             : #include <mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h>
+      19             : #include <mrs_uav_state_estimators/estimators/correction.h>
+      20             : 
+      21             : #include <mrs_uav_state_estimators/LateralEstimatorConfig.h>
+      22             : 
+      23             : //}
+      24             : 
+      25             : namespace mrs_uav_state_estimators
+      26             : {
+      27             : 
+      28             : namespace lat_generic
+      29             : {
+      30             : 
+      31             : const int n_states       = 6;
+      32             : const int n_inputs       = 2;
+      33             : const int n_measurements = 2;
+      34             : 
+      35             : }  // namespace lat_generic
+      36             : 
+      37             : class LatGeneric : public LateralEstimator<lat_generic::n_states> {
+      38             : 
+      39             :   typedef mrs_lib::DynamicReconfigureMgr<LateralEstimatorConfig> drmgr_t;
+      40             : 
+      41             :   using lkf_t         = mrs_lib::LKF<lat_generic::n_states, lat_generic::n_inputs, lat_generic::n_measurements>;
+      42             :   using varstep_lkf_t = mrs_lib::varstepLKF<lat_generic::n_states, lat_generic::n_inputs, lat_generic::n_measurements>;
+      43             :   using A_t           = lkf_t::A_t;
+      44             :   using B_t           = lkf_t::B_t;
+      45             :   using H_t           = lkf_t::H_t;
+      46             :   using Q_t           = lkf_t::Q_t;
+      47             :   using x_t           = lkf_t::x_t;
+      48             :   using P_t           = lkf_t::P_t;
+      49             :   using u_t           = lkf_t::u_t;
+      50             :   using z_t           = lkf_t::z_t;
+      51             :   using R_t           = lkf_t::R_t;
+      52             :   using statecov_t    = lkf_t::statecov_t;
+      53             : 
+      54             :   typedef mrs_lib::Repredictor<varstep_lkf_t> rep_lkf_t;
+      55             : 
+      56             : private:
+      57             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      58             : 
+      59             :   std::string parent_state_est_name_;
+      60             : 
+      61             :   double                                      dt_;
+      62             :   double                                      input_coeff_, default_input_coeff_;
+      63             :   A_t                                         A_;
+      64             :   B_t                                         B_;
+      65             :   H_t                                         H_;
+      66             :   Q_t                                         Q_;
+      67             :   std::shared_ptr<lkf_t>                      lkf_;
+      68             :   std::unique_ptr<rep_lkf_t>                  lkf_rep_;
+      69             :   std::vector<std::shared_ptr<varstep_lkf_t>> models_;
+      70             :   mutable std::mutex                          mutex_lkf_;
+      71             :   statecov_t                                  sc_;
+      72             :   mutable std::mutex                          mutex_sc_;
+      73             : 
+      74             :   std::unique_ptr<drmgr_t> drmgr_;
+      75             :   void                     callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level);
+      76             : 
+      77             :   z_t                innovation_;
+      78             :   mutable std::mutex mtx_innovation_;
+      79             : 
+      80             :   bool          is_error_state_first_time_ = true;
+      81             :   ros::Duration error_state_duration_;
+      82             :   ros::Time     prev_time_in_error_state_;
+      83             : 
+      84             :   bool is_repredictor_enabled_;
+      85             :   int  rep_buffer_size_ = 200;
+      86             : 
+      87             :   const bool is_core_plugin_;
+      88             : 
+      89             :   std::vector<std::string>                                              correction_names_;
+      90             :   std::vector<std::shared_ptr<Correction<lat_generic::n_measurements>>> corrections_;
+      91             : 
+      92             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput> sh_control_input_;
+      93             :   void                                                timeoutCallback(const std::string &topic, const ros::Time &last_msg);
+      94             :   std::atomic<bool>                                   is_input_ready_ = false;
+      95             : 
+      96             : 
+      97             :   std::function<std::optional<double>()>               fun_get_hdg_;
+      98             :   std::string                                          hdg_source_topic_;
+      99             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput> sh_hdg_state_;
+     100             :   std::atomic<bool>                                    is_hdg_state_ready_ = false;
+     101             : 
+     102             :   ros::Timer timer_update_;
+     103             :   void       timerUpdate(const ros::TimerEvent &event);
+     104             : 
+     105             :   ros::Timer timer_check_health_;
+     106             :   void       timerCheckHealth(const ros::TimerEvent &event);
+     107             : 
+     108             : 
+     109             :   void doCorrection(const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id);
+     110             :   void doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp);
+     111             : 
+     112             :   bool isConverged();
+     113             : 
+     114             :   Q_t                getQ();
+     115             :   mutable std::mutex mtx_Q_;
+     116             : 
+     117             : public:
+     118          99 :   LatGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin,
+     119             :              std::function<std::optional<double>()> fun_get_hdg)
+     120          99 :       : LateralEstimator<lat_generic::n_states>(name, ns_frame_id),
+     121             :         parent_state_est_name_(parent_state_est_name),
+     122             :         is_core_plugin_(is_core_plugin),
+     123          99 :         fun_get_hdg_(fun_get_hdg) {
+     124          99 :   }
+     125             : 
+     126         198 :   ~LatGeneric(void) {
+     127         198 :   }
+     128             : 
+     129             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     130             :   bool start(void) override;
+     131             :   bool pause(void) override;
+     132             :   bool reset(void) override;
+     133             : 
+     134             :   double getState(const int &state_idx_in) const override;
+     135             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     136             : 
+     137             :   void setState(const double &state_in, const int &state_idx_in) override;
+     138             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     139             : 
+     140             :   states_t getStates(void) const override;
+     141             :   void     setStates(const states_t &states_in) override;
+     142             : 
+     143             :   double getCovariance(const int &state_idx_in) const override;
+     144             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     145             : 
+     146             :   covariance_t getCovarianceMatrix(void) const override;
+     147             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     148             : 
+     149             :   double getInnovation(const int &state_idx) const override;
+     150             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     151             : 
+     152             :   void setDt(const double &dt);
+     153             :   void setInputCoeff(const double &input_coeff);
+     154             : 
+     155             :   void generateRepredictorModels(const double input_coeff);
+     156             : 
+     157             :   void generateA();
+     158             :   void generateB();
+     159             : 
+     160             :   std::string getNamespacedName() const;
+     161             : 
+     162             :   std::string getPrintName() const;
+     163             : };
+     164             : }  // namespace mrs_uav_state_estimators
+     165             : 
+     166             : #endif  // ESTIMATORS_LATERAL_LAT_GENERIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html new file mode 100644 index 0000000000..f10b1f5fd4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html @@ -0,0 +1,62 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6fa70431bbed0c82bdfc8693d0379281e7228df2 GIT binary patch literal 708 zcmV;#0z3VQP)00{{R3zw{NF0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpKi;y_O zOB%{}8Z7WkVq8Vb85a;TMu+qO%z?qqs6t!d`_Ki(-X4L2*k+}t7^nPr@-bN4TCPl_ zAGp6E9d7CK7+_eqUf?z7BcRy=1apRLR>%N^9({@URod!l4E(Mq!gNnhOh#%nr`xJF z0&gcCa`_Kj2fR~XXGX)?Of0kTg4lj1u0XNaX4Yj}%I_Dq9E>oi0UM9B2aPcLmq6+; z8SRj4#@J{W09l#!j6(GZlPQ;|JTn?&hqW1v0Y)_RlK8rk90QMZNdi37H%47_7c7NB zOri%l;k@Xz1ZB76BAWEk^e%km5%UsOeFKDr$-Ws)8B`WmUuTojJrVIpXJz-)Q?=Qo z_U~}{q)@~Zrvmbe*OZrB)>9@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::LateralEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html new file mode 100644 index 0000000000..9bb942fc76 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LateralEstimator<6>::LateralEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.frameset.html new file mode 100644 index 0000000000..421acedc4b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html new file mode 100644 index 0000000000..30525818d5 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_LATERAL_LATERAL_ESTIMATOR_H
+       2             : #define ESTIMATORS_LATERAL_LATERAL_ESTIMATOR_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <mrs_uav_state_estimators/estimators/partial_estimator.h>
+       7             : 
+       8             : //}
+       9             : 
+      10             : namespace mrs_uav_state_estimators
+      11             : {
+      12             : 
+      13             : namespace lateral
+      14             : {
+      15             : const char type[] = "LATERAL";
+      16             : const int  n_axes = 2;
+      17             : }  // namespace lateral
+      18             : 
+      19             : template <int n_states>
+      20             : class LateralEstimator : public PartialEstimator<n_states, lateral::n_axes> {
+      21             : 
+      22             : protected:
+      23          99 :   LateralEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, lateral::n_axes>(lateral::type, name, frame_id) {
+      24          99 :   }
+      25             : 
+      26          99 :   ~LateralEstimator(void) {
+      27          99 :   }
+      28             : 
+      29             : private:
+      30             :   static const int _n_axes_   = lateral::n_axes;
+      31             :   static const int _n_states_ = n_states;
+      32             :   static const int _n_inputs_;
+      33             :   static const int _n_measurements_;
+      34             : };
+      35             : 
+      36             : }  // namespace mrs_uav_state_estimation
+      37             : 
+      38             : #endif  // ESTIMATORS_LATERAL_LATERAL_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..e5433d137d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.overview.html @@ -0,0 +1,30 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..463fd4e692090c58e2404f8d94bb0a27bf35e9a1 GIT binary patch literal 284 zcmeAS@N?(olHy`uVBq!ia0vp^0YI$A!VDxAx-dEcDTx4|5ZC|z|E~gqo(Lf);IkB zKih&OP$28bu>u3%=xv5Up%7}X91nEC!(6qssq(S1+&l+1@+JxwXI-pyIterm(k zm#tmhJg2gI`;Ii4a?1#x-QnJ|iaF^W_vhMMmm^Nj4*76q=hq`i%l+g3ZEcx6r(A$z z@!KPZZk1oD-*7BGC5Pd7uj5YR+DC>{-?qhe>=g-rWL$nv%v)pLp`KFjGVv_k6V;t& ZA77ZC+PdV^7oe*dJYD@<);T3K0RRatb^`za literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html new file mode 100644 index 0000000000..9b1ac00ebe --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-04-25 05:42:57Functions:192479.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
void mrs_uav_state_estimators::PartialEstimator<2, 1>::publishInput<Eigen::Matrix<double, 1, 1, 0, 1, 1> >(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::PartialEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().299
mrs_uav_state_estimators::PartialEstimator<6, 2>::PartialEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().299
mrs_uav_state_estimators::PartialEstimator<3, 1>::PartialEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)172
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2172
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const141796
void mrs_uav_state_estimators::PartialEstimator<6, 2>::publishInput<Eigen::Matrix<double, 2, 1, 0, 2, 1> >(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&) const172067
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const172068
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const172071
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const172072
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const190558
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const190570
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const190583
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const297656
void mrs_uav_state_estimators::PartialEstimator<3, 1>::publishInput<Eigen::Matrix<double, 1, 1, 0, 1, 1> >(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&) const297743
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const297801
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const297982
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2587627
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html new file mode 100644 index 0000000000..830cf407a6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-04-25 05:42:57Functions:192479.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::PartialEstimator<2, 1>::PartialEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().299
mrs_uav_state_estimators::PartialEstimator<3, 1>::PartialEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)172
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2172
mrs_uav_state_estimators::PartialEstimator<6, 2>::PartialEstimator(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)99
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().299
void mrs_uav_state_estimators::PartialEstimator<2, 1>::publishInput<Eigen::Matrix<double, 1, 1, 0, 1, 1> >(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const190583
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const190570
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const190558
void mrs_uav_state_estimators::PartialEstimator<3, 1>::publishInput<Eigen::Matrix<double, 1, 1, 0, 1, 1> >(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&) const297743
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const297801
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const141796
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const297982
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const297656
void mrs_uav_state_estimators::PartialEstimator<6, 2>::publishInput<Eigen::Matrix<double, 2, 1, 0, 2, 1> >(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&) const172067
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const172071
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2587627
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const172072
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const172068
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.frameset.html new file mode 100644 index 0000000000..c351510cb2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html new file mode 100644 index 0000000000..1d40defc98 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html @@ -0,0 +1,264 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-04-25 05:42:57Functions:192479.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_PARTIAL_ESTIMATOR_H
+       2             : #define ESTIMATORS_PARTIAL_ESTIMATOR_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <Eigen/Dense>
+       9             : 
+      10             : #include <mrs_msgs/Float64Stamped.h>
+      11             : #include <mrs_msgs/Float64ArrayStamped.h>
+      12             : 
+      13             : #include <mrs_msgs/EstimatorOutput.h>
+      14             : 
+      15             : #include <mrs_uav_managers/estimation_manager/estimator.h>
+      16             : 
+      17             : //}
+      18             : 
+      19             : namespace mrs_uav_state_estimators
+      20             : {
+      21             : 
+      22             : typedef enum
+      23             : {
+      24             :   ELAND,
+      25             :   SWITCH,
+      26             :   MITIGATE,
+      27             :   NONE
+      28             : } ExcInnoAction_t;
+      29             : const int n_ExcInnoAction = 4;
+      30             : 
+      31             : const std::map<std::string, ExcInnoAction_t> map_exc_inno_action{{"eland", ExcInnoAction_t::ELAND},
+      32             :                                                         {"switch", ExcInnoAction_t::SWITCH},
+      33             :                                                         {"mitigate", ExcInnoAction_t::MITIGATE},
+      34             : {"none", ExcInnoAction_t::NONE}};
+      35             : 
+      36             : template <int n_states, int n_axes>
+      37             : class PartialEstimator : public mrs_uav_managers::Estimator {
+      38             : 
+      39             : public:
+      40             :   typedef Eigen::Matrix<double, n_states, 1>        states_t;
+      41             :   typedef Eigen::Matrix<double, n_states, n_states> covariance_t;
+      42             : 
+      43             : protected:
+      44             :   mutable mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>     ph_output_;
+      45             :   mutable mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped> ph_input_;
+      46             : 
+      47             :   bool first_iter_ = true;
+      48             : 
+      49             :   double pos_innovation_limit_;
+      50             :   ExcInnoAction_t exc_innovation_action_;
+      51             :   std::string exc_innovation_action_name_;
+      52             :   bool innovation_ok_ = true;
+      53             : 
+      54             : private:
+      55             :   static const int _n_axes_   = n_axes;
+      56             :   static const int _n_states_ = n_states;
+      57             :   static const int _n_inputs_;
+      58             :   static const int _n_measurements_;
+      59             : 
+      60             : public:
+      61         370 :   PartialEstimator(const std::string &type, const std::string &name, const std::string &frame_id) : Estimator(type, name, frame_id) {
+      62         370 :   }
+      63             : 
+      64         370 :   ~PartialEstimator(void) {
+      65         370 :   }
+      66             : 
+      67             :   //  methods
+      68             :   virtual double getState(const int &state_idx_in) const                    = 0;
+      69             :   virtual double getState(const int &state_id_in, const int &axis_in) const = 0;
+      70             : 
+      71             :   virtual void setState(const double &state_in, const int &state_idx_in)                    = 0;
+      72             :   virtual void setState(const double &state_in, const int &state_id_in, const int &axis_in) = 0;
+      73             : 
+      74             :   virtual states_t getStates(void) const                = 0;
+      75             :   virtual void     setStates(const states_t &states_in) = 0;
+      76             : 
+      77             :   virtual double getCovariance(const int &state_idx_in) const                    = 0;
+      78             :   virtual double getCovariance(const int &state_id_in, const int &axis_in) const = 0;
+      79             : 
+      80             :   virtual covariance_t getCovarianceMatrix(void) const                 = 0;
+      81             :   virtual void         setCovarianceMatrix(const covariance_t &cov_in) = 0;
+      82             : 
+      83             :   virtual double getInnovation(const int &state_idx) const                       = 0;
+      84             :   virtual double getInnovation(const int &state_id_in, const int &axis_in) const = 0;
+      85             : 
+      86             :   // implemented methods
+      87             :   // access methods
+      88             :   std::vector<double> getStatesAsVector(void) const;
+      89             :   std::vector<double> getCovarianceAsVector(void) const;
+      90             : 
+      91             :   int stateIdToIndex(const int &state_id_in, const int &axis_in) const;
+      92             : 
+      93             :   template <typename u_t>
+      94             :   void publishInput(const u_t &u, const ros::Time& stamp) const;
+      95             :   void publishOutput() const;
+      96             : };
+      97             : 
+      98             : /*//{ method implementations */
+      99             : 
+     100             : /*//{ getStatesAsvector() */
+     101             : template <int n_states, int n_axes>
+     102      660624 : std::vector<double> PartialEstimator<n_states, n_axes>::getStatesAsVector(void) const {
+     103      660624 :   const states_t      states = getStates();
+     104      660201 :   std::vector<double> states_vec;
+     105             :   /* for (auto st : Eigen::MatrixXd::Map(states, states.size(), 1).rowwise()) { */
+     106             :   /*   states_vec.push_back(*st); */
+     107             :   /* } */
+     108     2966213 :   for (int i = 0; i < states.size(); i++) {
+     109     2304849 :     states_vec.push_back(states(i));
+     110             :   }
+     111     1319922 :   return states_vec;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ getCovarianceAsvector() */
+     116             : template <int n_states, int n_axes>
+     117      660282 : std::vector<double> PartialEstimator<n_states, n_axes>::getCovarianceAsVector(void) const {
+     118      660282 :   const covariance_t  covariance = getCovarianceMatrix();
+     119      660139 :   std::vector<double> covariance_vec;
+     120             :   /* for (auto cov : covariance.reshaped<Eigen::RowMajor>(covariance.size())) { */
+     121             :   /*   covariance_vec.push_back(*cov); */
+     122             :   /* } */
+     123     2964825 :   for (int i = 0; i < covariance.rows(); i++) {
+     124    11938594 :     for (int j = 0; j < covariance.cols(); j++) {
+     125     9631980 :       covariance_vec.push_back(covariance(i, j));
+     126             :     }
+     127             :   }
+     128     1320238 :   return covariance_vec;
+     129             : }
+     130             : /*//}*/
+     131             : 
+     132             : /*//{ stateIdToIndex() */
+     133             : template <int n_states, int n_axes>
+     134     2729423 : int PartialEstimator<n_states, n_axes>::stateIdToIndex(const int &state_id_in, const int &axis_in) const {
+     135     2729423 :   return state_id_in * _n_axes_ + axis_in;
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ publishOutput() */
+     140             : template <int n_states, int n_axes>
+     141      660455 : void PartialEstimator<n_states, n_axes>::publishOutput() const {
+     142             : 
+     143      660455 :   if (!ch_->debug_topics.output) {
+     144           0 :     return;
+     145             :   }
+     146             : 
+     147     1320737 :   mrs_msgs::EstimatorOutput msg;
+     148      659845 :   msg.header.stamp    = ros::Time::now();
+     149      660701 :   msg.header.frame_id = getFrameId();
+     150      660579 :   msg.state           = getStatesAsVector();
+     151      659458 :   msg.covariance      = getCovarianceAsVector();
+     152             : 
+     153      660068 :   ph_output_.publish(msg);
+     154             : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ publishInput() */
+     158             : template <int n_states, int n_axes>
+     159             : template <typename u_t>
+     160      469810 : void PartialEstimator<n_states, n_axes>::publishInput(const u_t &u, const ros::Time& stamp) const {
+     161             : 
+     162      469810 :   if (!ch_->debug_topics.input) {
+     163           0 :     return;
+     164             :   }
+     165             : 
+     166      935588 :   mrs_msgs::Float64ArrayStamped msg;
+     167      465527 :   msg.header.stamp = stamp;
+     168     1105650 :   for (int i = 0; i < u.rows(); i++) {
+     169      635503 :     msg.values.push_back(u(i));
+     170             :   }
+     171             : 
+     172      466651 :   ph_input_.publish(msg);
+     173             : }
+     174             : /*//}*/
+     175             : 
+     176             : /*//}*/
+     177             : 
+     178             : }  // namespace mrs_uav_state_estimators
+     179             : 
+     180             : #endif  // ESTIMATORS_PARTIAL_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..53598245fe --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html @@ -0,0 +1,65 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..93d0059d99c5276a3d7504dec305b703e9c2544a GIT binary patch literal 820 zcmV-41Izr0P)#@C3`iKnVY{!^>+5Ti^XbD$B2m^_W4WoO!9duo z-Z{-(9>Tbpej>C6<5QG`Q7+>dxK-<6*Z26^G2WBM?U2rlwQ_sF$2}iC0|0T?a9!ck zg>`^>HmCq%_a91pWDKyA&VN=4)q}|BGh<*HIJKkuYKn{|reXw2nyRkHw7C)p9=|=8 zJf37-gC?-NI3Fb>v4-o~l*%hZ1|H`-6=~9h9*J&(k;+U6hE`fcpJ2VZX3tx*J~F4_ z6Fjok@yeitW}v`5P9f_8l%n83IAW0yE8emebLqF=f7bZh?LTVq3uFp|1t4+l1@=F1 ztt?uhL$*njS;MT?!ZhjtP5#@=E}-~KCQPG&Mu7rNnwV3r%M>8lavpo!J_zved3D+H zI$F{d)j;S2ODoMy#2h={=7dCRL$fY(AX4Y&-flD0o}gsJa?UZ`?RXU(XhtvXO0enC9pfUx%Sy + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
state_generic.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
passthrough.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
<unnamed>100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html new file mode 100644 index 0000000000..9fd82f0a84 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
passthrough.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
<unnamed>100.0 %4 / 4100.0 %3 / 3
state_generic.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html new file mode 100644 index 0000000000..cce978d43d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
passthrough.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
<unnamed>100.0 %4 / 4100.0 %3 / 3
state_generic.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html new file mode 100644 index 0000000000..e3bdf9f8f5 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
state_generic.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
passthrough.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html new file mode 100644 index 0000000000..b1cf88ab90 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
passthrough.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
state_generic.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html new file mode 100644 index 0000000000..6fb106e962 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-25 05:42:57Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
passthrough.h +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
state_generic.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html new file mode 100644 index 0000000000..6f2a3fe11b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::passthrough::Passthrough::Passthrough()1
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough()1
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough().21
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html new file mode 100644 index 0000000000..edafbbe45d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::passthrough::Passthrough::Passthrough()1
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough()1
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough().21
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.frameset.html new file mode 100644 index 0000000000..bf04eeb1cb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html new file mode 100644 index 0000000000..75b0804d80 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html @@ -0,0 +1,173 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-25 05:42:57Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_STATE_PASSTHROUGH_H
+       2             : #define ESTIMATORS_STATE_PASSTHROUGH_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <nav_msgs/Odometry.h>
+       9             : 
+      10             : #include <mrs_lib/lkf.h>
+      11             : #include <mrs_lib/profiler.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : #include <mrs_lib/subscribe_handler.h>
+      14             : #include <mrs_lib/publisher_handler.h>
+      15             : #include <mrs_lib/attitude_converter.h>
+      16             : #include <mrs_lib/transformer.h>
+      17             : 
+      18             : #include <mrs_uav_managers/state_estimator.h>
+      19             : 
+      20             : //}
+      21             : 
+      22             : namespace mrs_uav_state_estimators
+      23             : {
+      24             : 
+      25             : namespace passthrough
+      26             : {
+      27             : const char name[]         = "passthrough";
+      28             : const char frame_id[]     = "passthrough_origin";
+      29             : const char package_name[] = "mrs_uav_state_estimators";
+      30             : 
+      31             : const bool is_core_plugin = true;
+      32             : 
+      33             : using namespace mrs_uav_managers::estimation_manager;
+      34             : 
+      35             : class Passthrough : public mrs_uav_managers::StateEstimator {
+      36             : 
+      37             :   using CommonHandlers_t = mrs_uav_managers::estimation_manager::CommonHandlers_t;
+      38             : 
+      39             : private:
+      40             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      41             : 
+      42             :   const std::string est_lat_name_ = "lat_passthrough";
+      43             : 
+      44             :   const std::string est_alt_name_ = "alt_passthrough";
+      45             : 
+      46             :   const std::string est_hdg_name_ = "hdg_passthrough";
+      47             : 
+      48             :   const bool is_core_plugin_;
+      49             : 
+      50             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_passthrough_odom_;
+      51             :   double                                        _critical_timeout_passthrough_odom_;
+      52             :   std::string                                   msg_topic_;
+      53             : 
+      54             :   ros::Timer                 timer_update_;
+      55             :   void                       timerUpdate(const ros::TimerEvent &event);
+      56             :   nav_msgs::OdometryConstPtr prev_msg_;
+      57             :   bool                       first_iter_ = true;
+      58             :   ros::Timer                 timer_check_health_;
+      59             :   void                       timerCheckHealth(const ros::TimerEvent &event);
+      60             : 
+      61             :   bool isConverged();
+      62             : 
+      63             :   void waitForEstimationInitialization();
+      64             : 
+      65             : public:
+      66           1 :   Passthrough() : StateEstimator(passthrough::name, passthrough::frame_id, passthrough::package_name), is_core_plugin_(is_core_plugin) {
+      67           1 :   }
+      68             : 
+      69           2 :   ~Passthrough(void) {
+      70           2 :   }
+      71             : 
+      72             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+      73             :   bool start(void) override;
+      74             :   bool pause(void) override;
+      75             :   bool reset(void) override;
+      76             : 
+      77             :   /* mrs_msgs::UavState  getUavState() override; */
+      78             :   /* nav_msgs::Odometry  getInnovation() const override; */
+      79             :   /* std::vector<double> getPoseCovariance() const override; */
+      80             :   /* std::vector<double> getTwistCovariance() const override; */
+      81             : 
+      82             :   bool setUavState(const mrs_msgs::UavState &uav_state) override;
+      83             : };
+      84             : 
+      85             : }  // namespace passthrough
+      86             : 
+      87             : }  // namespace mrs_uav_state_estimators
+      88             : 
+      89             : #endif  // ESTIMATORS_STATE_PASSTHROUGH_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html new file mode 100644 index 0000000000..88d95237e0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html @@ -0,0 +1,43 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e44c5c845709562b6df9a8255e423f99ae0ac942 GIT binary patch literal 431 zcmV;g0Z{&lP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpfO*y3~prpJEFVmUx_krB1ErfufwoNp%&};z`6IVHe zcp7+wCJ?=_l=#9mnq$p<=SVIGyJdG{!>EreX941!LoLd-1=^Nd$^p8)B+kWgO+ibE+Soi~>yX!XLzaeKefFguZm-q|K zRsu&L69)Xr&V>2cwV0%$x?)mMi23O}bC|&P2;J)0x8LW + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::StateGeneric::~StateGeneric()0
mrs_uav_state_estimators::StateGeneric::StateGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)99
mrs_uav_state_estimators::StateGeneric::~StateGeneric().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html new file mode 100644 index 0000000000..44c675549e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::StateGeneric::StateGeneric(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)99
mrs_uav_state_estimators::StateGeneric::~StateGeneric()0
mrs_uav_state_estimators::StateGeneric::~StateGeneric().299
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.frameset.html new file mode 100644 index 0000000000..fa3e046124 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html new file mode 100644 index 0000000000..b13b5f06c5 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html @@ -0,0 +1,183 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef ESTIMATORS_STATE_STATE_GENERIC_H
+       2             : #define ESTIMATORS_STATE_STATE_GENERIC_H
+       3             : 
+       4             : /* includes //{ */
+       5             : 
+       6             : #include <ros/ros.h>
+       7             : 
+       8             : #include <nav_msgs/Odometry.h>
+       9             : 
+      10             : #include <mrs_lib/lkf.h>
+      11             : #include <mrs_lib/profiler.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : #include <mrs_lib/subscribe_handler.h>
+      14             : #include <mrs_lib/publisher_handler.h>
+      15             : #include <mrs_lib/attitude_converter.h>
+      16             : #include <mrs_lib/transformer.h>
+      17             : 
+      18             : #include <mrs_uav_managers/state_estimator.h>
+      19             : 
+      20             : #include <mrs_uav_state_estimators/estimators/lateral/lat_generic.h>
+      21             : #include <mrs_uav_state_estimators/estimators/altitude/alt_generic.h>
+      22             : #include <mrs_uav_state_estimators/estimators/heading/heading_estimator.h>
+      23             : #include <mrs_uav_state_estimators/estimators/heading/hdg_generic.h>
+      24             : #include <mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h>
+      25             : 
+      26             : //}
+      27             : 
+      28             : namespace mrs_uav_state_estimators
+      29             : {
+      30             : 
+      31             : namespace hdg_estimator
+      32             : {
+      33             : const int n_states = 2;
+      34             : }  // namespace hdg_estimator
+      35             : 
+      36             : namespace state_generic
+      37             : {
+      38             : const char package_name[] = "mrs_uav_state_estimators";
+      39             : }
+      40             : 
+      41             : class StateGeneric : public mrs_uav_managers::StateEstimator {
+      42             : 
+      43             : private:
+      44             :   std::unique_ptr<LatGeneric> est_lat_;
+      45             :   std::string                 est_lat_name_;
+      46             : 
+      47             :   std::unique_ptr<AltGeneric> est_alt_;
+      48             :   std::string                 est_alt_name_;
+      49             : 
+      50             :   bool                                                       is_hdg_passthrough_;
+      51             :   std::unique_ptr<HeadingEstimator<hdg_estimator::n_states>> est_hdg_;
+      52             :   std::string                                                est_hdg_name_;
+      53             : 
+      54             :   bool is_override_frame_id_;
+      55             : 
+      56             :   const bool is_core_plugin_;
+      57             : 
+      58             :   std::string                                                 topic_orientation_;
+      59             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_hw_api_orient_;
+      60             : 
+      61             :   std::string                                              topic_angular_velocity_;
+      62             :   mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped> sh_hw_api_ang_vel_;
+      63             : 
+      64             :   ros::Timer timer_update_;
+      65             :   void       timerUpdate(const ros::TimerEvent &event);
+      66             : 
+      67             :   ros::Timer timer_check_health_;
+      68             :   void       timerCheckHealth(const ros::TimerEvent &event);
+      69             : 
+      70             :   ros::Timer timer_pub_attitude_;
+      71             :   void       timerPubAttitude(const ros::TimerEvent &event);
+      72             : 
+      73             :   bool isConverged();
+      74             : 
+      75             :   void waitForEstimationInitialization();
+      76             : 
+      77             : public:
+      78          99 :   StateGeneric(const std::string &name, const bool is_core_plugin)
+      79          99 :       : StateEstimator(name, name + "_origin", state_generic::package_name), is_core_plugin_(is_core_plugin) {
+      80          99 :   }
+      81             : 
+      82          99 :   ~StateGeneric(void) {
+      83          99 :   }
+      84             : 
+      85             :   void initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+      86             :   bool start(void) override;
+      87             :   bool pause(void) override;
+      88             :   bool reset(void) override;
+      89             : 
+      90             :   bool setUavState(const mrs_msgs::UavState &uav_state) override;
+      91             : 
+      92             :   std::optional<double> getHeading() const;
+      93             : 
+      94             :   void updateUavState();
+      95             : };
+      96             : 
+      97             : }  // namespace mrs_uav_state_estimators
+      98             : 
+      99             : #endif  // ESTIMATORS_STATE_STATE_GENERIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.overview.html new file mode 100644 index 0000000000..e38bed99a7 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.overview.html @@ -0,0 +1,45 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ce4b3647eeeb837a14b19ec628cfa1df6eb92a02 GIT binary patch literal 438 zcmV;n0ZIOeP)Vz^Tt*~F@W@P<*6F01U09LCdGFD)a8j6o=J2p{;vfxW zTmvR#CNa*UrHl>IXwIO>SPp5cm|>sc#=+(ZuNG^GMUk{ zQBea#8B8mCE@mK|!s=en1Db;rbN#@)cC_5>SiyMbUa9H0Ar&dg*Fr-I gkFgQlPPSmrA6JucjdQ-?RR91007*qoM6N<$f^ZwfOaK4? literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html new file mode 100644 index 0000000000..fb432b31db --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13218471.7 %
Date:2024-04-25 05:42:57Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
<unnamed>68.3 %28 / 4133.3 %2 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
<unnamed>78.9 %45 / 5737.5 %3 / 8
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
<unnamed>72.5 %29 / 4066.7 %4 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
<unnamed>70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html new file mode 100644 index 0000000000..e14831ea1c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13218471.7 %
Date:2024-04-25 05:42:57Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
<unnamed>68.3 %28 / 4133.3 %2 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
<unnamed>70.0 %7 / 1075.0 %6 / 8
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
<unnamed>72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
<unnamed>78.9 %45 / 5737.5 %3 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html new file mode 100644 index 0000000000..a682f3c704 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13218471.7 %
Date:2024-04-25 05:42:57Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
<unnamed>68.3 %28 / 4133.3 %2 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
<unnamed>72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
<unnamed>78.9 %45 / 5737.5 %3 / 8
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
<unnamed>70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html new file mode 100644 index 0000000000..51c7b18257 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13218471.7 %
Date:2024-04-25 05:42:57Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html new file mode 100644 index 0000000000..e76672b2b6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13218471.7 %
Date:2024-04-25 05:42:57Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html new file mode 100644 index 0000000000..69e5f22d26 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13218471.7 %
Date:2024-04-25 05:42:57Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html new file mode 100644 index 0000000000..41c79daca6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_excessive_tilt.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:284168.3 %
Date:2024-04-25 05:42:57Functions:2633.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcExcessiveTilt<1>::reset()0
mrs_uav_state_estimators::ProcExcessiveTilt<2>::reset()0
mrs_uav_state_estimators::ProcExcessiveTilt<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
mrs_uav_state_estimators::ProcExcessiveTilt<2>::ProcExcessiveTilt(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
mrs_uav_state_estimators::ProcExcessiveTilt<1>::ProcExcessiveTilt(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)77
mrs_uav_state_estimators::ProcExcessiveTilt<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)140025
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html new file mode 100644 index 0000000000..ef37d1184d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_excessive_tilt.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:284168.3 %
Date:2024-04-25 05:42:57Functions:2633.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcExcessiveTilt<1>::reset()0
mrs_uav_state_estimators::ProcExcessiveTilt<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)140025
mrs_uav_state_estimators::ProcExcessiveTilt<1>::ProcExcessiveTilt(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)77
mrs_uav_state_estimators::ProcExcessiveTilt<2>::reset()0
mrs_uav_state_estimators::ProcExcessiveTilt<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
mrs_uav_state_estimators::ProcExcessiveTilt<2>::ProcExcessiveTilt(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.frameset.html new file mode 100644 index 0000000000..73cb0ae2b8 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html new file mode 100644 index 0000000000..06a6d9d758 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html @@ -0,0 +1,206 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_excessive_tilt.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:284168.3 %
Date:2024-04-25 05:42:57Functions:2633.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef PROCESSORS_PROC_EXCESSIVE_TILT_H
+       3             : #define PROCESSORS_PROC_EXCESSIVE_TILT_H
+       4             : 
+       5             : #include <mrs_uav_state_estimators/processors/processor.h>
+       6             : 
+       7             : #include <mrs_lib/param_loader.h>
+       8             : #include <mrs_lib/subscribe_handler.h>
+       9             : #include <mrs_lib/attitude_converter.h>
+      10             : 
+      11             : #include <Eigen/Dense>
+      12             : 
+      13             : #include <limits>
+      14             : 
+      15             : namespace mrs_uav_state_estimators
+      16             : {
+      17             : 
+      18             : using namespace mrs_uav_managers::estimation_manager;
+      19             : 
+      20             : template <int n_measurements>
+      21             : class ProcExcessiveTilt : public Processor<n_measurements> {
+      22             : 
+      23             : public:
+      24             :   typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+      25             : 
+      26             : public:
+      27             :   ProcExcessiveTilt(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      28             :                     const std::shared_ptr<PrivateHandlers_t>& ph);
+      29             : 
+      30             :   std::tuple<bool, bool> process(measurement_t& measurement) override;
+      31             :   void                   reset();
+      32             : 
+      33             : private:
+      34             :   double max_tilt_sq_;
+      35             : 
+      36             :   std::string                                                 orientation_topic_;
+      37             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_;
+      38             : };
+      39             : 
+      40             : /*//{ constructor */
+      41             : template <int n_measurements>
+      42          77 : ProcExcessiveTilt<n_measurements>::ProcExcessiveTilt(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+      43             :                                                      const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
+      44          77 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      45             : 
+      46             :   // | --------------------- load parameters -------------------- |
+      47          77 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      48             : 
+      49          77 :   ph->param_loader->loadParam("orientation_topic", orientation_topic_);
+      50             :   double max_tilt;
+      51          77 :   ph->param_loader->loadParam("max_tilt", max_tilt);
+      52             : 
+      53          77 :   max_tilt = M_PI * (max_tilt / 180.0);
+      54             : 
+      55          77 :   max_tilt_sq_ = std::pow(max_tilt, 2);
+      56             : 
+      57          77 :   if (!ph->param_loader->loadedSuccessfully()) {
+      58           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
+      59           0 :     ros::shutdown();
+      60             :   }
+      61             : 
+      62             :   // | -------------- initialize subscribe handlers ------------- |
+      63          77 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64          77 :   shopts.nh                 = nh;
+      65          77 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66          77 :   shopts.no_message_timeout = ros::Duration(1.0);
+      67          77 :   shopts.threadsafe         = true;
+      68          77 :   shopts.autostart          = true;
+      69          77 :   shopts.queue_size         = 10;
+      70          77 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72          77 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch->uav_name + "/" + orientation_topic_);
+      73          77 : }
+      74             : /*//}*/
+      75             : 
+      76             : /*//{ process() */
+      77             : template <int n_measurements>
+      78      140025 : std::tuple<bool, bool> ProcExcessiveTilt<n_measurements>::process([[maybe_unused]] measurement_t& measurement) {
+      79             : 
+      80      140025 :   if (!Processor<n_measurements>::enabled_) {
+      81           0 :     return {true, true};
+      82             :   }
+      83             : 
+      84      140025 :   if (!sh_orientation_.hasMsg()) {
+      85           0 :     return {false, false};
+      86             :   }
+      87             : 
+      88      140028 :   bool ok_flag     = true;
+      89      140028 :   bool should_fuse = true;
+      90             : 
+      91             :   try {
+      92      140028 :     Eigen::Matrix3d orientation_R = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion);
+      93             : 
+      94      140023 :     const double tilt = mrs_lib::geometry::angleBetween(Eigen::Vector3d(0, 0, 1), orientation_R.col(2));
+      95             : 
+      96      140027 :     const bool is_excessive_tilt = std::pow(tilt, 2) > max_tilt_sq_;
+      97             : 
+      98      140000 :     if (is_excessive_tilt) {
+      99           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: excessive tilt of %.2f deg. Not fusing correction.", Processor<n_measurements>::getPrintName().c_str(), tilt / M_PI * 180);
+     100           0 :       ok_flag     = false;
+     101           0 :       should_fuse = false;
+     102             :     }
+     103             :   }
+     104           0 :   catch (...) {
+     105           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed obtaining tilt value", Processor<n_measurements>::getPrintName().c_str());
+     106           0 :     ok_flag     = false;
+     107           0 :     should_fuse = false;
+     108             :   }
+     109      140025 :   return {ok_flag, should_fuse};
+     110             : }
+     111             : /*//}*/
+     112             : 
+     113             : /*//{ reset() */
+     114             : template <int n_measurements>
+     115           0 : void ProcExcessiveTilt<n_measurements>::reset() {
+     116             :   // no need to do anything
+     117           0 : }
+     118             : /*//}*/
+     119             : 
+     120             : }  // namespace mrs_uav_state_estimators
+     121             : 
+     122             : #endif  // PROCESSORS_PROC_EXCESSIVE_TILT_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html new file mode 100644 index 0000000000..8e05401d3b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html @@ -0,0 +1,51 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b64a252cab730e64e7e46966d4a4e6187ae9facd GIT binary patch literal 656 zcmV;B0&o3^P)2i$Eh$_a1ffbCT**)NUE|Y6jd4E=SUmi7Q$-F96_Fewyni(}s z8H0^6tmnSunwqp;B?-iQBu~`s;jK0u*OC-l;Z5Ys$lp==NYoYb-EkG;j$#=aohb^A zxVIw9f>6Q96v!N|V`^ZI1H|4neH=Q$4G>C#U=C;w|JZ<$n#o3HGK^`ZTPo(aw=>Y% zzB1#x?l9&V0|MQ>Ul@IbO5jqACP8pC?h0+-6628-4W2XVGHK_b0-on5*kxbW=QcA2t_n<dA`)n{kiFQy@)L%bU+m{=J%6!JwI^2f)b#rDD!qJ%x^-mM`xIlI q49GSm>wgvaPj~7ls(-H<<@^E?wHgS_z&(2a0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-04-25 05:42:57Functions:2633.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcMedianFilter<1>::reset()0
mrs_uav_state_estimators::ProcMedianFilter<2>::reset()0
mrs_uav_state_estimators::ProcMedianFilter<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
mrs_uav_state_estimators::ProcMedianFilter<2>::ProcMedianFilter(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
mrs_uav_state_estimators::ProcMedianFilter<1>::ProcMedianFilter(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)77
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)140028
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html new file mode 100644 index 0000000000..074b6d2a73 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-04-25 05:42:57Functions:2633.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcMedianFilter<1>::reset()0
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)140028
mrs_uav_state_estimators::ProcMedianFilter<1>::ProcMedianFilter(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)77
mrs_uav_state_estimators::ProcMedianFilter<2>::reset()0
mrs_uav_state_estimators::ProcMedianFilter<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
mrs_uav_state_estimators::ProcMedianFilter<2>::ProcMedianFilter(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.frameset.html new file mode 100644 index 0000000000..6b6962ed34 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html new file mode 100644 index 0000000000..42b9f656bc --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html @@ -0,0 +1,192 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-04-25 05:42:57Functions:2633.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef PROCESSORS_PROC_MEDIAN_FILTER_H
+       3             : #define PROCESSORS_PROC_MEDIAN_FILTER_H
+       4             : 
+       5             : #include <mrs_uav_state_estimators/processors/processor.h>
+       6             : 
+       7             : #include <mrs_lib/median_filter.h>
+       8             : #include <mrs_lib/param_loader.h>
+       9             : 
+      10             : #include <limits>
+      11             : 
+      12             : namespace mrs_uav_state_estimators
+      13             : {
+      14             : 
+      15             : using namespace mrs_uav_managers::estimation_manager;
+      16             : 
+      17             : template <int n_measurements>
+      18             : class ProcMedianFilter : public Processor<n_measurements> {
+      19             : 
+      20             : public:
+      21             :   typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+      22             : 
+      23             : public:
+      24             :   ProcMedianFilter(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      25             :                    const std::shared_ptr<PrivateHandlers_t>& ph);
+      26             : 
+      27             :   std::tuple<bool, bool> process(measurement_t& measurement) override;
+      28             :   void                   reset();
+      29             : 
+      30             : private:
+      31             :   std::vector<mrs_lib::MedianFilter> vec_mf_;
+      32             :   int                                buffer_size_;
+      33             :   double                             max_diff_;
+      34             : };
+      35             : 
+      36             : /*//{ constructor */
+      37             : template <int n_measurements>
+      38          77 : ProcMedianFilter<n_measurements>::ProcMedianFilter(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+      39             :                                                    const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
+      40          77 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      41             : 
+      42             :   // | --------------------- load parameters -------------------- |
+      43          77 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      44             : 
+      45          77 :   ph->param_loader->loadParam("buffer_size", buffer_size_);
+      46          77 :   ph->param_loader->loadParam("max_diff", max_diff_);
+      47             : 
+      48          77 :   if (!ph->param_loader->loadedSuccessfully()) {
+      49           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
+      50           0 :     ros::shutdown();
+      51             :   }
+      52             : 
+      53             :   // min and max values are not checked by median filter, so set them to limits of double
+      54          77 :   const double min_valid = std::numeric_limits<double>::lowest();
+      55          77 :   const double max_valid = std::numeric_limits<double>::max();
+      56             : 
+      57             :   // initialize median filter
+      58         154 :   for (int i = 0; i < n_measurements; i++) {
+      59          77 :     vec_mf_.push_back(mrs_lib::MedianFilter(buffer_size_, min_valid, max_valid, max_diff_));
+      60             :   }
+      61          77 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      140028 : std::tuple<bool, bool> ProcMedianFilter<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68      140028 :   if (!Processor<n_measurements>::enabled_) {
+      69           0 :     return {true, true};
+      70             :   }
+      71             : 
+      72      140028 :   bool ok_flag     = true;
+      73      140028 :   bool should_fuse = true;
+      74      280039 :   for (int i = 0; i < measurement.rows(); i++) {
+      75      139989 :     vec_mf_[i].add(measurement(i));
+      76      140007 :     if (vec_mf_[i].full()) {
+      77      132386 :       if (!vec_mf_[i].check(measurement(i))) {
+      78           0 :         std::stringstream ss_measurement_string;
+      79           0 :         ss_measurement_string << measurement(i);
+      80           0 :         ss_measurement_string << " ";
+      81           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: measurement[%d]: %s declined by median filter (median: %.2f, max_diff: %.2f).",
+      82             :                           Processor<n_measurements>::getPrintName().c_str(), i, ss_measurement_string.str().c_str(), vec_mf_[i].median(), max_diff_);
+      83           0 :         ok_flag     = false;
+      84           0 :         should_fuse = false;
+      85             :       }
+      86             :     } else {
+      87        7623 :       ROS_WARN_THROTTLE(1.0, "[%s]: median filter not full yet", Processor<n_measurements>::getPrintName().c_str());
+      88        7623 :       ok_flag     = false;
+      89        7623 :       should_fuse = false;
+      90             :     }
+      91             :   }
+      92             : 
+      93      140008 :   return {ok_flag, should_fuse};
+      94             : }
+      95             : /*//}*/
+      96             : 
+      97             : /*//{ reset() */
+      98             : template <int n_measurements>
+      99           0 : void ProcMedianFilter<n_measurements>::reset() {
+     100           0 :   for (auto mf : vec_mf_) {
+     101           0 :     mf.clear();
+     102             :   }
+     103           0 : }
+     104             : /*//}*/
+     105             : 
+     106             : }  // namespace mrs_uav_state_estimators
+     107             : 
+     108             : #endif  // PROCESSORS_PROC_MEDIAN_FILTER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html new file mode 100644 index 0000000000..99218c587d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html @@ -0,0 +1,47 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6923330e664ef60437c8a503a26089bc19866de0 GIT binary patch literal 635 zcmV->0)+jEP)|!_b`VE3^Lar_duazn?*P542Yg*&@;(Y2Dc4#e6hU;_|6R+NifR9fC38Z1>2d zrafk7b^)|k)UBv>lh(@J0r&mP=ECXD!yoWFU&8|g+@~h?V-6_58_JdWh~r7YY=j$` ze}+6-$gm+2U~77&zy}EzNm43lYtl11FdJj61!-HNE|Hp)4-!Y*xE0#=tI0hwaawf3 z8%E8pAiUVuz?-w@GC{gbz3ds=CnB|iOMUU5TS2F2{opi6rerWVnll+N zkR>#)AKzG#BeSO1rcBHDK1Uswfi}Iir!vbP?Hb4%Q-ARJp<$-x@P^dvLHfzR)yCkh z#{e=1=~=p@RcbG@ApOuPQIbOo~h_YFnh{sDB+ V&znIPT!#Pv002ovPDHLkV1lH}D0lz> literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html new file mode 100644 index 0000000000..e63d087280 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-04-25 05:42:57Functions:4666.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcSaturate<1>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)4
mrs_uav_state_estimators::ProcSaturate<1>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)79
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)3830
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)141804
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html new file mode 100644 index 0000000000..0b8d886d90 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-04-25 05:42:57Functions:4666.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcSaturate<1>::reset()0
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)141804
mrs_uav_state_estimators::ProcSaturate<1>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)79
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)3830
mrs_uav_state_estimators::ProcSaturate<2>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)4
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html new file mode 100644 index 0000000000..5f4af983f1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html new file mode 100644 index 0000000000..8efcfd082f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html @@ -0,0 +1,202 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-04-25 05:42:57Functions:4666.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef PROCESSORS_PROC_SATURATE_H
+       3             : #define PROCESSORS_PROC_SATURATE_H
+       4             : 
+       5             : #include <mrs_uav_state_estimators/processors/processor.h>
+       6             : 
+       7             : #include <mrs_lib/param_loader.h>
+       8             : 
+       9             : #include <limits>
+      10             : #include <functional>
+      11             : 
+      12             : namespace mrs_uav_state_estimators
+      13             : {
+      14             : 
+      15             : using namespace mrs_uav_managers::estimation_manager;
+      16             : 
+      17             : template <int n_measurements>
+      18             : class ProcSaturate : public Processor<n_measurements> {
+      19             : 
+      20             : public:
+      21             :   typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+      22             : 
+      23             : public:
+      24             :   ProcSaturate(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      25             :                const std::shared_ptr<PrivateHandlers_t>& ph, StateId_t state_id, std::function<double(int, int)> fun_get_state);
+      26             : 
+      27             :   std::tuple<bool, bool> process(measurement_t& measurement) override;
+      28             :   void                   reset();
+      29             : 
+      30             : private:
+      31             :   const StateId_t                 state_id_;
+      32             :   std::function<double(int, int)> fun_get_state_;
+      33             : 
+      34             :   bool   keep_enabled_;
+      35             :   double saturate_min_;
+      36             :   double saturate_max_;
+      37             :   double innovation_limit_;
+      38             : };
+      39             : 
+      40             : /*//{ constructor */
+      41             : template <int n_measurements>
+      42          83 : ProcSaturate<n_measurements>::ProcSaturate(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+      43             :                                            const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph, const StateId_t state_id,
+      44             :                                            std::function<double(int, int)> fun_get_state)
+      45          83 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48          83 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      49             : 
+      50          83 :   ph->param_loader->loadParam("start_enabled", this->start_enabled_);
+      51          83 :   this->enabled_ = this->start_enabled_;
+      52          83 :   ph->param_loader->loadParam("keep_enabled", keep_enabled_);
+      53          83 :   ph->param_loader->loadParam("min", saturate_min_);
+      54          83 :   ph->param_loader->loadParam("max", saturate_max_);
+      55          83 :   ph->param_loader->loadParam("limit", innovation_limit_);
+      56             : 
+      57          83 :   if (!ph->param_loader->loadedSuccessfully()) {
+      58           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
+      59           0 :     ros::shutdown();
+      60             :   }
+      61          83 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      145634 : std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68             :   // if no saturation is required, processing is successful
+      69      145634 :   if (!this->enabled_) {
+      70           0 :     return {true, true};
+      71             :   }
+      72             : 
+      73      145634 :   bool ok_flag     = true;
+      74      145634 :   bool should_fuse = true;
+      75      289819 :   for (int i = 0; i < measurement.rows(); i++) {
+      76      149450 :     const double state = fun_get_state_(state_id_, i);
+      77      149457 :     ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);
+      78             : 
+      79      149458 :     if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
+      80        5258 :       return {true, true};  // do not even try to saturate, trigger innovation-based switch to other estimator
+      81             :     }
+      82             : 
+      83      144178 :     if (measurement(i) > state + saturate_max_) {
+      84        1044 :       const double saturated = state + saturate_max_;
+      85        1044 :       ROS_WARN_THROTTLE(1.0, "[%s]: state[%d][%d]: %.2f, measurement[%d]: %.2f saturated to: %.2f.", Processor<n_measurements>::getPrintName().c_str(),
+      86             :                         state_id_, i, state, i, measurement(i), saturated);
+      87        1044 :       measurement(i) = saturated;
+      88        1044 :       ok_flag        = false;
+      89        1044 :       should_fuse    = true;
+      90      143142 :     } else if (measurement(i) < state + saturate_min_) {
+      91           0 :       const double saturated = state + saturate_min_;
+      92           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: state[%d][%d]: %.2f, measurement[%d]: %.2f saturated to: %.2f.", Processor<n_measurements>::getPrintName().c_str(),
+      93             :                         state_id_, i, state, i, measurement(i), saturated);
+      94           0 :       measurement(i) = saturated;
+      95           0 :       ok_flag        = false;
+      96           0 :       should_fuse    = true;
+      97             :     }
+      98             :   }
+      99             : 
+     100             :   // measurements are close to the state, no need to saturate until triggered externally again
+     101      140365 :   if (!this->keep_enabled_ && ok_flag) {
+     102           0 :     this->enabled_ = false;
+     103             :   }
+     104             : 
+     105      140365 :   return {ok_flag, should_fuse};  // saturated measurement is valid
+     106             : }
+     107             : /*//}*/
+     108             : 
+     109             : /*//{ reset() */
+     110             : template <int n_measurements>
+     111           0 : void ProcSaturate<n_measurements>::reset() {
+     112             :   // no need to reset anything
+     113           0 : }
+     114             : /*//}*/
+     115             : 
+     116             : }  // namespace mrs_uav_state_estimators
+     117             : 
+     118             : #endif  // PROCESSORS_PROC_MEDIAN_FILTER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.overview.html new file mode 100644 index 0000000000..174df6dcc3 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.overview.html @@ -0,0 +1,50 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..9849f98e369e50987231e01d23a49867fcf9bec5 GIT binary patch literal 711 zcmV;&0yzDNP)PuAm$E_Fv^0!cQUyv9+o8t9s_bg#@ixmc=cLnHvF_eGOC_ zIC=d+pc#F7gezUL=r-izK6S^=VOz^- zoTyaRs(gREA>hUbqo%&|$VMlDoalkatPzZW+4a<;8psABfO>hZ^Mt%Mz-{|Ww&S+{ z9p@(>r*AyLfZNc_c8mc92))RAgAwwNfKTgsTGr3&x>KQIGCeA;wB$OrpygAHn&rC5 zukaXCxKlpgNuk<2$#QnE&@aCnqtE)yfqy{9M(&-32O^%(+KM|qYtXE zrX~pb0OB-yBpN*L@iN4g3^{_@nsrt4AT#|K;mSiX7BvL61LR6V{h6t7ctXc2OVI|h zbLNpK?=(n2yo|tmsftIKOh{TZYe+yl#Ug+kR@agH=DyR8CX0!H#Zozz2Apm=e;R%l z0gn8YFyx$hB+9$$O+dVi!22RE9*+ndG=NRnDt_{Imfkbz0XIJNyV$HPd+0q|yjTk< zmEAsRgp7l6{iZ*rx}q>WKf;*PT-Gm|ue_N^y`!S@HB-N6)(Oumq9=Hq_P8IzClI4d t%w`5<@?i>{ZT~FL-+iN_Y2I%;xPLjP^YBqA0G9v&002ovPDHLkV1gH|QM>>E literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html new file mode 100644 index 0000000000..66dfaf44ff --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-04-25 05:42:57Functions:3837.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcTfToWorld<1>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::ProcTfToWorld<1>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)0
mrs_uav_state_estimators::ProcTfToWorld<1>::ProcTfToWorld(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::ProcTfToWorld(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)95
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)68314
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)191448
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html new file mode 100644 index 0000000000..27b8202852 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-04-25 05:42:57Functions:3837.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcTfToWorld<1>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::ProcTfToWorld<1>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)0
mrs_uav_state_estimators::ProcTfToWorld<1>::ProcTfToWorld(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)68314
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)191448
mrs_uav_state_estimators::ProcTfToWorld<2>::ProcTfToWorld(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)95
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.frameset.html new file mode 100644 index 0000000000..6aab5842a6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html new file mode 100644 index 0000000000..55b91b2671 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html @@ -0,0 +1,241 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-04-25 05:42:57Functions:3837.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef PROCESSORS_PROC_TF_TO_WORLD_H
+       3             : #define PROCESSORS_PROC_TF_TO_WORLD_H
+       4             : 
+       5             : #include <mrs_uav_state_estimators/processors/processor.h>
+       6             : 
+       7             : #include <sensor_msgs/NavSatFix.h>
+       8             : 
+       9             : #include <mrs_lib/gps_conversions.h>
+      10             : #include <mrs_lib/subscribe_handler.h>
+      11             : #include <mrs_lib/param_loader.h>
+      12             : 
+      13             : namespace mrs_uav_state_estimators
+      14             : {
+      15             : 
+      16             : using namespace mrs_uav_managers::estimation_manager;
+      17             : 
+      18             : template <int n_measurements>
+      19             : class ProcTfToWorld : public Processor<n_measurements> {
+      20             : 
+      21             : public:
+      22             :   typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+      23             : 
+      24             : public:
+      25             :   ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      26             :                 const std::shared_ptr<PrivateHandlers_t>& ph);
+      27             : 
+      28             :   std::tuple<bool, bool> process(measurement_t& measurement) override;
+      29             :   void                   reset();
+      30             : 
+      31             : private:
+      32             :   bool is_initialized_ = false;
+      33             : 
+      34             :   std::string gnss_topic_;
+      35             : 
+      36             :   std::mutex       mtx_gnss_;
+      37             :   std::atomic_bool got_gnss_                  = false;
+      38             :   std::atomic_bool is_gnss_offset_calculated_ = false;
+      39             :   double           gnss_x_;
+      40             :   double           gnss_y_;
+      41             : 
+      42             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+      43             :   void                                              callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg);
+      44             : };
+      45             : 
+      46             : /*//{ constructor */
+      47             : template <int n_measurements>
+      48          95 : ProcTfToWorld<n_measurements>::ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+      49             :                                              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
+      50          95 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      51             : 
+      52          95 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      53             : 
+      54          95 :   ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+      55             : 
+      56          95 :   gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+      57             : 
+      58          95 :   if (!ph->param_loader->loadedSuccessfully()) {
+      59           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
+      60           0 :     ros::shutdown();
+      61             :   }
+      62             : 
+      63             :   // | -------------- initialize subscribe handlers ------------- |
+      64          95 :   mrs_lib::SubscribeHandlerOptions shopts;
+      65          95 :   shopts.nh                 = nh;
+      66          95 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      67          95 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      68          95 :   shopts.threadsafe         = true;
+      69          95 :   shopts.autostart          = true;
+      70          95 :   shopts.queue_size         = 10;
+      71          95 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      72             : 
+      73          95 :   sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcTfToWorld::callbackGnss, this);
+      74             : 
+      75          95 :   is_initialized_ = true;
+      76          95 : }
+      77             : /*//}*/
+      78             : 
+      79             : /*//{ callbackGnss() */
+      80             : template <int n_measurements>
+      81       68314 : void ProcTfToWorld<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+      82             : 
+      83       68314 :   if (!is_initialized_) {
+      84       68172 :     return;
+      85             :   }
+      86             : 
+      87       68314 :   if (got_gnss_) {
+      88       68172 :     return;
+      89             :   }
+      90             : 
+      91          95 :   if (!std::isfinite(msg->latitude)) {
+      92           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->latitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+      93           0 :     return;
+      94             :   }
+      95             : 
+      96          95 :   if (!std::isfinite(msg->longitude)) {
+      97           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->longitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+      98           0 :     return;
+      99             :   }
+     100             : 
+     101         190 :   std::scoped_lock lock(mtx_gnss_);
+     102          95 :   mrs_lib::UTM(msg->latitude, msg->longitude, &gnss_x_, &gnss_y_);
+     103          95 :   ROS_INFO("[%s]: First GNSS obtained: %.2f %.2f", Processor<n_measurements>::getPrintName().c_str(), gnss_x_, gnss_y_);
+     104          95 :   got_gnss_ = true;
+     105             : }
+     106             : /*//}*/
+     107             : 
+     108             : /*//{ process() */
+     109             : template <int n_measurements>
+     110      191448 : std::tuple<bool, bool> ProcTfToWorld<n_measurements>::process(measurement_t& measurement) {
+     111             : 
+     112      191448 :   if (!Processor<n_measurements>::enabled_) {
+     113           0 :     return {true, true};
+     114             :   }
+     115             : 
+     116      382867 :   std::scoped_lock lock(mtx_gnss_);
+     117             : 
+     118      191441 :   if (!got_gnss_) {
+     119        7321 :     ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+     120        7321 :     return {false, false};
+     121             :   }
+     122             : 
+     123      184089 :   if (!is_gnss_offset_calculated_) {
+     124             : 
+     125          95 :     ROS_INFO_THROTTLE(1.0, "[%s]: debug: gnss_x_: %.2f, measurement(0): %.2f, world_origin.x: %.2f", Processor<n_measurements>::getPrintName().c_str(), gnss_x_,
+     126             :                       measurement(0), Processor<n_measurements>::ch_->world_origin.x);
+     127          95 :     ROS_INFO_THROTTLE(1.0, "[%s]: debug: gnss_y_: %.2f, measurement(1): %.2f, world_origin.y: %.2f", Processor<n_measurements>::getPrintName().c_str(), gnss_y_,
+     128             :                       measurement(1), Processor<n_measurements>::ch_->world_origin.y);
+     129          95 :     gnss_x_                    = (gnss_x_ - measurement(0)) - Processor<n_measurements>::ch_->world_origin.x;
+     130          95 :     gnss_y_                    = (gnss_y_ - measurement(1)) - Processor<n_measurements>::ch_->world_origin.y;
+     131          95 :     is_gnss_offset_calculated_ = true;
+     132          95 :     ROS_INFO_THROTTLE(1.0, "[%s]: GNSS world offset calculated as: [%.2f %.2f]", Processor<n_measurements>::getPrintName().c_str(), gnss_x_, gnss_y_);
+     133             :   }
+     134             : 
+     135      184101 :   measurement(0) += gnss_x_;
+     136      184039 :   measurement(1) += gnss_y_;
+     137             : 
+     138      184092 :   if (measurement(0) > 10000 || measurement(0) < -10000 || measurement(1) > 10000 || measurement(1) < -10000) {
+     139           0 :     ROS_WARN_THROTTLE(
+     140             :         1.0,
+     141             :         "[%s]: debug: Not expected to fly further than 10 km. This is most likely a bug. measurement(0): %.2f measurement(1): %.2f gnss_x_: %.2f gnss_y_: %.2f",
+     142             :         Processor<n_measurements>::getPrintName().c_str(), measurement(0), measurement(1), gnss_x_, gnss_y_);
+     143             :   }
+     144      184136 :   return {true, true};
+     145             : }
+     146             : /*//}*/
+     147             : 
+     148             : /*//{ reset() */
+     149             : template <int n_measurements>
+     150           0 : void ProcTfToWorld<n_measurements>::reset() {
+     151           0 :   got_gnss_                  = false;
+     152           0 :   is_gnss_offset_calculated_ = false;
+     153           0 : }
+     154             : /*//}*/
+     155             : }  // namespace mrs_uav_state_estimators
+     156             : 
+     157             : #endif  // PROCESSORS_PROC_TF_TO_WORLD_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html new file mode 100644 index 0000000000..c44e2f6583 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html @@ -0,0 +1,60 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a324c1257ee9037e3fabf82919209845d495e4b0 GIT binary patch literal 777 zcmV+k1NQuhP)YeKPLfI%d(hdQDZ?M6>}3P z7RRBNhCIV~)54mKVqgP#1>TJ!GMcesrS7XLRhvTlJ-0ACX3Ubb8gmW3ep|KBYVL9} zHd-<7`;=p5I<1j5^pje73r1fJMaKwP|cqki8fe9uJfx;%Wbp_7N?c1i- zs@QF_{E-s~SCSwed~Wz?gRvB~){V2)?OgR8#$YEf;2a}>+b`Au2g(_3%}QGU_x&fx z;r8R_b#C!I{JIMSxOYYEuRb6FCn{HKGMaFJ=!Hy?0as+k1)&-66k6a>E;AEZ;L4wF zan%6)Zj})NV?%eTdI$umGoG|`1`<#x$Y`sjMpN*{;DWZp2<+Wtz^zY|f%0-py;1ik zkBLUwjN#i&Dm@6@IIgQKKx!36Tpq`0W6_nq&_R}6o3E|AfdR&8jjrC4u{Ct0GH1mn@RJ^unROSIn z8T0jue2Ov0(yxHhqK~ZpCV|9C+el>18JoPqcO>uJM{*ssdR4s?ih)8QW#%Gdft*0R z<-$p4uEB(~JE*Wz;QNNbE~)Wo;y-Qt-3Y9U&PVx|$~D((KRkav&y1%T`8*R}J(;}9 z#;$y-)Ai$Mtwhw$s5(I5?6E7^s^ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-04-25 05:42:57Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Processor<1>::toggle(bool)0
mrs_uav_state_estimators::Processor<2>::toggle(bool)0
mrs_uav_state_estimators::Processor<2>::Processor(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const102
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const165
mrs_uav_state_estimators::Processor<1>::Processor(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)233
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const310
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const504
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html new file mode 100644 index 0000000000..52ea97cf18 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-04-25 05:42:57Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Processor<1>::toggle(bool)0
mrs_uav_state_estimators::Processor<1>::Processor(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)233
mrs_uav_state_estimators::Processor<2>::toggle(bool)0
mrs_uav_state_estimators::Processor<2>::Processor(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const165
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const310
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const504
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const102
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.frameset.html new file mode 100644 index 0000000000..7ea1b3521d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html new file mode 100644 index 0000000000..f8cbc9f1ae --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-04-25 05:42:57Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef PROCESSORS_PROCESSOR_H
+       3             : #define PROCESSORS_PROCESSOR_H
+       4             : 
+       5             : namespace mrs_uav_state_estimators
+       6             : {
+       7             : 
+       8             : using namespace mrs_uav_managers::estimation_manager;
+       9             : 
+      10             : template <int n_measurements>
+      11             : class Processor {
+      12             : 
+      13             : public:
+      14             :   typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+      15             : 
+      16             : public:
+      17             :   std::string getName() const;
+      18             :   std::string getNamespacedName() const;
+      19             :   std::string getPrintName() const;
+      20             : 
+      21             :   void toggle(bool enable);
+      22             : 
+      23             :   virtual std::tuple<bool, bool> process(measurement_t& measurement) = 0;
+      24             :   virtual void                   reset()                             = 0;
+      25             : 
+      26             : protected:
+      27         332 :   Processor([[maybe_unused]] ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      28             :             const std::shared_ptr<PrivateHandlers_t>& ph)
+      29         332 :       : correction_name_(correction_name), name_(name), ch_(ch), ph_(ph) {
+      30         332 :   }  // protected constructor to prevent instantiation
+      31             : 
+      32             :   const std::string correction_name_;
+      33             :   const std::string name_;
+      34             : 
+      35             :   const std::shared_ptr<CommonHandlers_t>  ch_;
+      36             :   const std::shared_ptr<PrivateHandlers_t> ph_;
+      37             : 
+      38             :   bool enabled_       = true;
+      39             :   bool start_enabled_ = true;
+      40             : };
+      41             : 
+      42             : /*//{ getName() */
+      43             : template <int n_measurements>
+      44             : std::string Processor<n_measurements>::getName() const {
+      45             :   return name_;
+      46             : }
+      47             : /*//}*/
+      48             : 
+      49             : /*//{ getNamespacedName() */
+      50             : template <int n_measurements>
+      51         412 : std::string Processor<n_measurements>::getNamespacedName() const {
+      52         412 :   return correction_name_ + "/" + name_;
+      53             : }
+      54             : /*//}*/
+      55             : 
+      56             : /*//{ getPrintName() */
+      57             : template <int n_measurements>
+      58         669 : std::string Processor<n_measurements>::getPrintName() const {
+      59         669 :   return ch_->nodelet_name + "/" + correction_name_ + "/" + name_;
+      60             : }
+      61             : /*//}*/
+      62             : 
+      63             : /*//{ toggle() */
+      64             : template <int n_measurements>
+      65           0 : void Processor<n_measurements>::toggle(bool enable) {
+      66           0 :   enabled_ = enable;
+      67           0 : }
+      68             : /*//}*/
+      69             : 
+      70             : }  // namespace mrs_uav_state_estimators
+      71             : 
+      72             : #endif  // PROCESSORS_PROCESSOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html new file mode 100644 index 0000000000..804fb09cf2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html @@ -0,0 +1,38 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4a4f97a2d3dc93342019c73afac4a3056273be23 GIT binary patch literal 436 zcmV;l0ZaagP)h`Wz7!Rbg$du*y>R3?p)aIa0!P*vNNhPKFvCxC5v{Rl z)_t110j;ZuETrnnuHU&viBrZ79BzXdKV9)a-f^E-ve$EC5zK@+k?0DgW0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/agl - garmin_agl.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::garmin_agl::GarminAgl::isConverged()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::pause()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::reset()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::getHeightCovariance() const0
(anonymous namespace)::ProxyExec0::ProxyExec0()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)73
mrs_uav_state_estimators::garmin_agl::GarminAgl::start()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)1436
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)139277
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const141549
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html new file mode 100644 index 0000000000..c8ba2785a9 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/agl - garmin_agl.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)73
mrs_uav_state_estimators::garmin_agl::GarminAgl::isConverged()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)139277
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)1436
mrs_uav_state_estimators::garmin_agl::GarminAgl::pause()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::reset()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::start()73
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const141549
mrs_uav_state_estimators::garmin_agl::GarminAgl::getHeightCovariance() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.frameset.html new file mode 100644 index 0000000000..bd433a3da0 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html new file mode 100644 index 0000000000..cb0bf7de5f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html @@ -0,0 +1,318 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/agl - garmin_agl.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <mrs_uav_state_estimators/estimators/agl/garmin_agl.h>
+       4             : 
+       5             : //}
+       6             : 
+       7             : namespace mrs_uav_state_estimators
+       8             : {
+       9             : 
+      10             : namespace garmin_agl
+      11             : {
+      12             : 
+      13             : /* initialize() //{*/
+      14          73 : void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          73 :   ch_ = ch;
+      17          73 :   ph_ = ph;
+      18          73 :   nh_ = nh;
+      19             : 
+      20          73 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      21             : 
+      22             :   // | --------------------- load parameters -------------------- |
+      23             : 
+      24          73 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          73 :   if (is_core_plugin_) {
+      27             : 
+      28          73 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      29          73 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32          73 :   if (!ph->param_loader->loadedSuccessfully()) {
+      33           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      34           0 :     ros::shutdown();
+      35             :   }
+      36             : 
+      37             :   // | ------------------ timers initialization ----------------- |
+      38          73 :   _update_timer_rate_       = 100;                                                                                          // TODO: parametrize
+      39          73 :   timer_update_             = nh.createTimer(ros::Rate(_update_timer_rate_), &GarminAgl::timerUpdate, this, false, false);  // not running after init
+      40          73 :   _check_health_timer_rate_ = 1;                                                                                            // TODO: parametrize
+      41          73 :   timer_check_health_       = nh.createTimer(ros::Rate(_check_health_timer_rate_), &GarminAgl::timerCheckHealth, this);
+      42             : 
+      43             :   // | --------------- subscribers initialization --------------- |
+      44         146 :   mrs_lib::SubscribeHandlerOptions shopts;
+      45          73 :   shopts.nh                 = nh;
+      46          73 :   shopts.node_name          = getPrintName();
+      47          73 :   shopts.no_message_timeout = ros::Duration(0.5);
+      48          73 :   shopts.threadsafe         = true;
+      49          73 :   shopts.autostart          = true;
+      50          73 :   shopts.queue_size         = 10;
+      51          73 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      52             : 
+      53             :   // | ---------------- publishers initialization --------------- |
+      54          73 :   ph_agl_height_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, Support::toSnakeCase(getName()) + "/agl_height", 10);
+      55          73 :   if (ch_->debug_topics.covariance) {
+      56           0 :     ph_agl_height_cov_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, Support::toSnakeCase(getName()) + "/agl_height_cov", 10);
+      57             :   }
+      58          73 :   if (ch_->debug_topics.diag) {
+      59           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, Support::toSnakeCase(getName()) + "/diagnostics", 10);
+      60             :   }
+      61             : 
+      62             :   // | ---------------- estimators initialization --------------- |
+      63             : 
+      64          73 :   est_agl_garmin_ = std::make_unique<AltGeneric>(est_agl_name_, frame_id_, getName(), is_core_plugin_);
+      65          73 :   est_agl_garmin_->initialize(nh, ch_, ph_);
+      66             : 
+      67          73 :   max_flight_z_ = est_agl_garmin_->getMaxFlightZ();
+      68             : 
+      69             :   // | ------------------ initialize published messages ------------------ |
+      70          73 :   agl_height_init_.header.frame_id     = ns_frame_id_;
+      71          73 :   agl_height_cov_init_.header.frame_id = ns_frame_id_;
+      72             : 
+      73             :   // | ------------------ finish initialization ----------------- |
+      74             : 
+      75          73 :   if (changeState(INITIALIZED_STATE)) {
+      76          73 :     ROS_INFO("[%s]: Estimator initialized", getPrintName().c_str());
+      77             :   } else {
+      78           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+      79             :   }
+      80          73 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84          73 : bool GarminAgl::start(void) {
+      85             : 
+      86             : 
+      87          73 :   if (isInState(READY_STATE)) {
+      88             : 
+      89             :     bool est_agl_garmin_start_successful;
+      90             : 
+      91          73 :     if (est_agl_garmin_->isStarted() || est_agl_garmin_->isRunning()) {
+      92           0 :       est_agl_garmin_start_successful = true;
+      93             :     } else {
+      94          73 :       est_agl_garmin_start_successful = est_agl_garmin_->start();
+      95             :     }
+      96             : 
+      97          73 :     if (est_agl_garmin_start_successful) {
+      98          73 :       timer_update_.start();
+      99          73 :       changeState(STARTED_STATE);
+     100          73 :       return true;
+     101             :     }
+     102             : 
+     103             :   } else {
+     104           0 :     ROS_WARN("[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     105           0 :     ros::Duration(1.0).sleep();
+     106             :   }
+     107           0 :   return false;
+     108             : 
+     109             :   ROS_ERROR("[%s]: Failed to start", getPrintName().c_str());
+     110             :   return false;
+     111             : }
+     112             : /*//}*/
+     113             : 
+     114             : /*//{ pause() */
+     115           0 : bool GarminAgl::pause(void) {
+     116             : 
+     117           0 :   if (isInState(RUNNING_STATE)) {
+     118           0 :     est_agl_garmin_->pause();
+     119           0 :     changeState(STOPPED_STATE);
+     120           0 :     return true;
+     121             : 
+     122             :   } else {
+     123           0 :     return false;
+     124             :   }
+     125             : }
+     126             : /*//}*/
+     127             : 
+     128             : /*//{ reset() */
+     129           0 : bool GarminAgl::reset(void) {
+     130             : 
+     131           0 :   if (!isInitialized()) {
+     132           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     133           0 :     return false;
+     134             :   }
+     135             : 
+     136           0 :   est_agl_garmin_->pause();
+     137           0 :   changeState(STOPPED_STATE);
+     138             : 
+     139           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     140             : 
+     141           0 :   return true;
+     142             : }
+     143             : /*//}*/
+     144             : 
+     145             : /* timerUpdate() //{*/
+     146      139277 : void GarminAgl::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     147             : 
+     148      139277 :   if (!isInitialized()) {
+     149           0 :     return;
+     150             :   }
+     151             : 
+     152      139277 :   const ros::Time time_now = ros::Time::now();
+     153             : 
+     154      278554 :   mrs_msgs::Float64Stamped agl_height = agl_height_init_;
+     155      139277 :   agl_height.header.stamp             = time_now;
+     156      139277 :   agl_height.value                    = est_agl_garmin_->getState(POSITION);
+     157             : 
+     158      278554 :   mrs_msgs::Float64ArrayStamped agl_height_cov;
+     159      139277 :   agl_height_cov.header.stamp = time_now;
+     160             : 
+     161      139277 :   const int n_states = 2;  // TODO this should be defined somewhere else
+     162      139277 :   agl_height_cov.values.resize(n_states * n_states);
+     163      139277 :   agl_height_cov.values.at(n_states * POSITION + POSITION) = est_agl_garmin_->getCovariance(POSITION);
+     164      139277 :   agl_height_cov.values.at(n_states * VELOCITY + VELOCITY) = est_agl_garmin_->getCovariance(VELOCITY);
+     165             : 
+     166      139277 :   mrs_lib::set_mutexed(mtx_agl_height_, agl_height, agl_height_);
+     167      139277 :   mrs_lib::set_mutexed(mtx_agl_height_cov_, agl_height_cov, agl_height_cov_);
+     168             : 
+     169      139277 :   publishAglHeight();
+     170      139277 :   publishCovariance();
+     171      139277 :   publishDiagnostics();
+     172             : }
+     173             : /*//}*/
+     174             : 
+     175             : /*//{ timerCheckHealth() */
+     176        1436 : void GarminAgl::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     177             : 
+     178        1436 :   if (!isInitialized()) {
+     179           0 :     return;
+     180             :   }
+     181             : 
+     182        1436 :   if (isInState(INITIALIZED_STATE)) {
+     183             : 
+     184          79 :     if (est_agl_garmin_->isReady()) {
+     185          73 :       changeState(READY_STATE);
+     186          73 :       ROS_INFO("[%s]: Estimator is ready to start", getPrintName().c_str());
+     187             :     } else {
+     188           6 :       ROS_INFO("[%s]: %s subestimators to be ready", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     189           6 :       return;
+     190             :     }
+     191             :   }
+     192             : 
+     193             : 
+     194        1430 :   if (isInState(STARTED_STATE)) {
+     195          74 :     ROS_INFO("[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     196             : 
+     197          74 :     if (est_agl_garmin_->isRunning()) {
+     198          73 :       ROS_INFO("[%s]: Subestimators converged", getPrintName().c_str());
+     199          73 :       changeState(RUNNING_STATE);
+     200             :     } else {
+     201           1 :       return;
+     202             :     }
+     203             :   }
+     204             : }
+     205             : /*//}*/
+     206             : 
+     207             : /*//{ isConverged() */
+     208           0 : bool GarminAgl::isConverged() {
+     209             : 
+     210             :   // TODO: check convergence by rate of change of determinant
+     211             :   // most likely not used in top-level estimator
+     212             : 
+     213           0 :   return true;
+     214             : }
+     215             : /*//}*/
+     216             : 
+     217             : /*//{ getUavAglHeight() */
+     218      141549 : mrs_msgs::Float64Stamped GarminAgl::getUavAglHeight() const {
+     219      141549 :   return mrs_lib::get_mutexed(mtx_agl_height_, agl_height_);
+     220             : }
+     221             : /*//}*/
+     222             : 
+     223             : /*//{ getHeightCovariance() */
+     224           0 : std::vector<double> GarminAgl::getHeightCovariance() const {
+     225           0 :   return mrs_lib::get_mutexed(mtx_agl_height_cov_, agl_height_cov_.values);
+     226             : }
+     227             : /*//}*/
+     228             : 
+     229             : }  // namespace garmin_agl
+     230             : 
+     231             : }  // namespace mrs_uav_state_estimators
+     232             : 
+     233             : #include <pluginlib/class_list_macros.h>
+     234          73 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::garmin_agl::GarminAgl, mrs_uav_managers::AglEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html new file mode 100644 index 0000000000..a5ae0c148f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html @@ -0,0 +1,79 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..60a6dabc6a79013ab0baab9a996cb41e797fba75 GIT binary patch literal 1043 zcmV+u1nm2XP)mlAH zm%c|SA#y_F#Ao_rlyFoO@a>j?=??G%rd?QMHwe~hTZp#C-cKMxF-0hAj5Xf@GUd|f z=BeXv_cAxa*LZmxWWwU(o^HwE7^^Za&k)W1oQzD2wQd918i_$dE@?MLE|3xD$Mk?7 zD-5rAKy2ITWxt>Md0db1-e2z<0Jt?pY)2aqfqVfplZjxmpjqs?pUYX+-WR;e~rF0Zl;TH4?T<(_Nt9PUcZs zM#e-SQ($ByD>Ay8j44`Wx|;fQKo*|+e$v9vLTs9_i%cI!!|;gdnF7#uI9pVlZ#5pk{T9h^BSm>=?-!J`ln@tLo%U%dknI z#T&f6@=7Tio0=N?AdU|d@5?mlnZ#>-#InB*lFVG0+u))(O#K#J%_R}HcaP}p%?ZC zSgpo6@ZGZ((iPOM6qRBRgATnKxR%`YVp(wp!R+yv zz8kszemOfU8kuDemGpylC*O}7S|n(l87Z2kx$N<*=#P;pgQ>tEB%k4bo;?kHh!nfu zq@+r8f<62PBEuFY!8gGq_>doQWj@idW>zC3w>H6a{(;?!M&NGfUq$|_fpcm$|-hwjOlDt1StcW>i8UR zz_bb8Hrn_$yxTg5NYus%9NC^sfVp6TPRsys(vQiP;n3Lt0=LH-Qz2~cb)jiCu&I)N zcKy^c>?>MJcfFcf_?6GYxc{x1G24p#S?4Mn8WF6aF>B9gnEi>WLNVJjNMrb}%e{g_ z*VY63^c^@rzW+ N002ovPDHLkV1gh<_Cx>x literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-f.html new file mode 100644 index 0000000000..1bfa00fcf6 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.cpp +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
<unnamed>72.1 %75 / 10460.0 %6 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-l.html new file mode 100644 index 0000000000..0647f8343a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.cpp +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
<unnamed>72.1 %75 / 10460.0 %6 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/index-detail.html b/mrs_uav_state_estimators/src/estimators/agl/index-detail.html new file mode 100644 index 0000000000..f2db871a40 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.cpp +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
<unnamed>72.1 %75 / 10460.0 %6 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/agl/index-sort-f.html new file mode 100644 index 0000000000..33467769ab --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.cpp +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/agl/index-sort-l.html new file mode 100644 index 0000000000..999f73acd5 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.cpp +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/index.html b/mrs_uav_state_estimators/src/estimators/agl/index.html new file mode 100644 index 0000000000..014b41c92a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-25 05:42:57Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.cpp +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..09561b8ec5 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::setCovarianceMatrix(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::AltGeneric::pause()0
mrs_uav_state_estimators::AltGeneric::reset()0
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)172
mrs_uav_state_estimators::AltGeneric::isConverged()172
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)172
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)266
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)334
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const1111
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1345
mrs_uav_state_estimators::AltGeneric::start()4077
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(int, int)#1}::operator()(int, int) const141805
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const141810
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const183559
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const297728
mrs_uav_state_estimators::AltGeneric::getStates() const297907
mrs_uav_state_estimators::AltGeneric::setDt(double const&)297946
mrs_uav_state_estimators::AltGeneric::generateA()298489
mrs_uav_state_estimators::AltGeneric::generateB()298504
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)338486
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t)#2}::operator()(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t) const459308
mrs_uav_state_estimators::AltGeneric::doCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double, mrs_uav_managers::estimation_manager::StateId_t const&, ros::Time const&)459355
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)459373
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const645652
mrs_uav_state_estimators::AltGeneric::getState(int const&) const1095205
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html new file mode 100644 index 0000000000..e7a6bfd18a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)172
mrs_uav_state_estimators::AltGeneric::isConverged()172
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)338486
mrs_uav_state_estimators::AltGeneric::doCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double, mrs_uav_managers::estimation_manager::StateId_t const&, ros::Time const&)459355
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)459373
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)334
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)172
mrs_uav_state_estimators::AltGeneric::setCovarianceMatrix(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::AltGeneric::pause()0
mrs_uav_state_estimators::AltGeneric::reset()0
mrs_uav_state_estimators::AltGeneric::setDt(double const&)297946
mrs_uav_state_estimators::AltGeneric::start()4077
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)266
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::generateA()298489
mrs_uav_state_estimators::AltGeneric::generateB()298504
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1345
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const645652
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const183559
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const1111
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const297728
mrs_uav_state_estimators::AltGeneric::getState(int const&) const1095205
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const141810
mrs_uav_state_estimators::AltGeneric::getStates() const297907
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t)#2}::operator()(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t) const459308
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(int, int)#1}::operator()(int, int) const141805
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html new file mode 100644 index 0000000000..be84a0e587 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html new file mode 100644 index 0000000000..7aa650b1ed --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html @@ -0,0 +1,839 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #define VERSION "0.0.6.0"
+       2             : 
+       3             : /* includes //{ */
+       4             : 
+       5             : #include <mrs_uav_state_estimators/estimators/altitude/alt_generic.h>
+       6             : 
+       7             : //}
+       8             : 
+       9             : namespace mrs_uav_state_estimators
+      10             : 
+      11             : {
+      12             : 
+      13             : /* initialize() //{*/
+      14         172 : void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16         172 :   ch_ = ch;
+      17         172 :   ph_ = ph;
+      18             : 
+      19         172 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // clang-format off
+      22         172 :   dt_ = 0.01;
+      23         172 :   input_coeff_ = 10;
+      24         172 :   default_input_coeff_ = 10;
+      25             : 
+      26         172 :   generateA();
+      27         172 :   generateB();
+      28             : 
+      29         172 :   H_ <<
+      30         172 :     1, 0, 0;
+      31             : 
+      32         172 :   innovation_ << 
+      33             :     0;
+      34             : 
+      35             :   // clang-format on
+      36             : 
+      37             :   // | --------------- initialize parameter loader -------------- |
+      38             : 
+      39         172 :   if (is_core_plugin_) {
+      40             : 
+      41         172 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      42         172 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      43             :   }
+      44             : 
+      45         172 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48             : 
+      49         172 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      50         172 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      51         172 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      52         172 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      53         172 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      54         172 :   if (is_repredictor_enabled_) {
+      55           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      56             :   }
+      57             : 
+      58             :   // | --------------- corrections initialization --------------- |
+      59         172 :   ph->param_loader->loadParam("corrections", correction_names_);
+      60             : 
+      61         423 :   for (auto corr_name : correction_names_) {
+      62         251 :     corrections_.push_back(std::make_shared<Correction<alt_generic::n_measurements>>(
+      63      142307 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::ALTITUDE, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      64      459308 :         [this](const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      65      459308 :           return this->doCorrection(meas, R, state);
+      66             :         }));
+      67             :   }
+      68             : 
+      69         172 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      70             : 
+      71             :   // | ----------- initialize process noise covariance ---------- |
+      72         172 :   Q_ = Q_t::Zero();
+      73             :   double tmp_noise;
+      74         172 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      75         172 :   Q_(POSITION, POSITION) = tmp_noise;
+      76         172 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      77         172 :   Q_(VELOCITY, VELOCITY) = tmp_noise;
+      78         172 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      79         172 :   Q_(ACCELERATION, ACCELERATION) = tmp_noise;
+      80             : 
+      81             :   // | ------- check if all parameters loaded successfully ------ |
+      82         172 :   if (!ph->param_loader->loadedSuccessfully()) {
+      83           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      84           0 :     ros::shutdown();
+      85             :   }
+      86             : 
+      87             :   // | ------------- initialize dynamic reconfigure ------------- |
+      88             :   drmgr_ =
+      89         172 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&AltGeneric::callbackReconfigure, this, _1, _2));
+      90         172 :   drmgr_->config.pos = Q_(POSITION, POSITION);
+      91         172 :   drmgr_->config.vel = Q_(VELOCITY, VELOCITY);
+      92         172 :   drmgr_->config.acc = Q_(ACCELERATION, ACCELERATION);
+      93         172 :   drmgr_->update_config(drmgr_->config);
+      94             : 
+      95             :   // | --------------- Kalman filter intialization -------------- |
+      96         172 :   const x_t        x0 = x_t::Zero();
+      97         172 :   const P_t        P0 = 1e3 * P_t::Identity();
+      98         172 :   const statecov_t sc0({x0, P0});
+      99         172 :   sc_ = sc0;
+     100             : 
+     101         172 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     102         172 :   if (is_repredictor_enabled_) {
+     103             : 
+     104           0 :     generateRepredictorModels(input_coeff_);
+     105             : 
+     106           0 :     const u_t       u0 = u_t::Zero();
+     107           0 :     const ros::Time t0 = ros::Time::now();
+     108           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
+     109             : 
+     110           0 :     setDt(1.0 / ch_->desired_uav_state_rate);
+     111             :   }
+     112             : 
+     113             :   // | ------------------ timers initialization ----------------- |
+     114         172 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &AltGeneric::timerUpdate, this);  // not running after init
+     115             :   /* timer_check_health_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &AltGeneric::timerCheckHealth, this); */
+     116             : 
+     117             :   // | --------------- subscribers initialization --------------- |
+     118             :   // subscriber to odometry
+     119         344 :   mrs_lib::SubscribeHandlerOptions shopts;
+     120         172 :   shopts.nh                 = nh;
+     121         172 :   shopts.node_name          = getPrintName();
+     122         172 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     123         172 :   shopts.threadsafe         = true;
+     124         172 :   shopts.autostart          = true;
+     125         172 :   shopts.queue_size         = 10;
+     126         172 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     127             : 
+     128         172 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     129             : 
+     130             :   // | ---------------- publishers initialization --------------- |
+     131         172 :   if (ch_->debug_topics.input) {
+     132         172 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     133             :   }
+     134         172 :   if (ch_->debug_topics.output) {
+     135         172 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     136             :   }
+     137         172 :   if (ch_->debug_topics.diag) {
+     138           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+     139             :   }
+     140             : 
+     141             :   // | ------------------ finish initialization ----------------- |
+     142             : 
+     143         172 :   if (changeState(INITIALIZED_STATE)) {
+     144         172 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+     145             :   } else {
+     146           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+     147             :   }
+     148         172 : }
+     149             : /*//}*/
+     150             : 
+     151             : /*//{ start() */
+     152        4077 : bool AltGeneric::start(void) {
+     153             : 
+     154        4077 :   if (isInState(READY_STATE)) {
+     155             :     /* timer_update_.start(); */
+     156         172 :     changeState(STARTED_STATE);
+     157         172 :     return true;
+     158             : 
+     159             :   } else {
+     160        3905 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     161        3905 :     return false;
+     162             :   }
+     163             : }
+     164             : /*//}*/
+     165             : 
+     166             : /*//{ pause() */
+     167           0 : bool AltGeneric::pause(void) {
+     168             : 
+     169           0 :   if (isInState(RUNNING_STATE)) {
+     170           0 :     changeState(STOPPED_STATE);
+     171           0 :     return true;
+     172             : 
+     173             :   } else {
+     174           0 :     return false;
+     175             :   }
+     176             : }
+     177             : /*//}*/
+     178             : 
+     179             : /*//{ reset() */
+     180           0 : bool AltGeneric::reset(void) {
+     181             : 
+     182           0 :   if (!isInitialized()) {
+     183           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     184           0 :     return false;
+     185             :   }
+     186             : 
+     187           0 :   changeState(STOPPED_STATE);
+     188             : 
+     189             :   // Initialize LKF state and covariance
+     190           0 :   const x_t        x0 = x_t::Zero();
+     191           0 :   const P_t        P0 = 1e6 * P_t::Identity();
+     192           0 :   const statecov_t sc0({x0, P0});
+     193           0 :   sc_ = sc0;
+     194             : 
+     195             :   // Instantiate the LKF itself
+     196           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     197           0 :   if (is_repredictor_enabled_) {
+     198             : 
+     199           0 :     const u_t       u0 = u_t::Zero();
+     200           0 :     const ros::Time t0 = ros::Time(0);
+     201           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
+     202             :   }
+     203             : 
+     204           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     205             : 
+     206           0 :   return true;
+     207             : }
+     208             : /*//}*/
+     209             : 
+     210             : /* timerUpdate() //{*/
+     211      338486 : void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
+     212             : 
+     213             : 
+     214      338486 :   if (!isInitialized()) {
+     215       40605 :     return;
+     216             :   }
+     217             : 
+     218      338386 :   switch (getCurrentSmState()) {
+     219             : 
+     220           0 :     case UNINITIALIZED_STATE: {
+     221           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     222           0 :       break;
+     223             :     }
+     224             : 
+     225        5727 :     case READY_STATE: {
+     226        5727 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     227        5726 :       break;
+     228             :     }
+     229             : 
+     230        8247 :     case INITIALIZED_STATE: {
+     231             :       // initialize the estimator with current corrections
+     232        8515 :       for (auto correction : corrections_) {
+     233        8338 :         auto res = correction->getProcessedCorrection();
+     234        8329 :         if (res) {
+     235         266 :           auto measurement_stamped = res.value();
+     236         266 :           setState(measurement_stamped.value(0), correction->getStateId());
+     237         266 :           ROS_INFO("[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     238             :         } else {
+     239        8075 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     240             :                             correction->getNamespacedName().c_str());
+     241        8068 :           return;
+     242             :         }
+     243             :       }
+     244         171 :       changeState(READY_STATE);
+     245         172 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     246         172 :       break;
+     247             :     }
+     248             : 
+     249         172 :     case STARTED_STATE: {
+     250         172 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     251             : 
+     252         172 :       if (isConverged()) {
+     253         172 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     254         172 :         changeState(RUNNING_STATE);
+     255             :       }
+     256         172 :       break;
+     257             :     }
+     258             : 
+     259      324330 :     case RUNNING_STATE: {
+     260      800856 :       for (auto correction : corrections_) {
+     261      476395 :         if (!correction->isHealthy()) {
+     262           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     263           0 :           changeState(ERROR_STATE);
+     264             :         }
+     265             :       }
+     266      324348 :       break;
+     267             :     }
+     268             : 
+     269           0 :     case STOPPED_STATE: {
+     270           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     271           0 :       break;
+     272             :     }
+     273             : 
+     274           0 :     case ERROR_STATE: {
+     275           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     276           0 :       ros::Time t_now = ros::Time::now();
+     277           0 :       if (is_error_state_first_time_) {
+     278           0 :         prev_time_in_error_state_  = t_now;
+     279           0 :         is_error_state_first_time_ = false;
+     280           0 :         error_state_duration_      = ros::Duration(0.0);
+     281             :       }
+     282           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     283             : 
+     284             : 
+     285             :       // check if all corrections are healthy now
+     286           0 :       bool all_corrections_healthy = true;
+     287           0 :       for (auto correction : corrections_) {
+     288           0 :         if (!correction->isHealthy()) {
+     289           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     290           0 :           all_corrections_healthy = false;
+     291             :         }
+     292             :       }
+     293             : 
+     294           0 :       if (all_corrections_healthy && innovation_ok_) {
+     295             :         // initialize the estimator again if corrections become healthy
+     296           0 :         if (error_state_duration_.toSec() > 5.0) {
+     297           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     298           0 :           changeState(INITIALIZED_STATE);
+     299           0 :           is_error_state_first_time_ = true;
+     300           0 :         }
+     301             :       } else {
+     302           0 :         is_error_state_first_time_ = true;
+     303             :       }
+     304             : 
+     305           0 :       prev_time_in_error_state_ = t_now;
+     306             : 
+     307           0 :       break;
+     308             :     }
+     309             :   }
+     310             : 
+     311      330425 :   if (sh_control_input_.newMsg()) {
+     312      157116 :     is_input_ready_ = true;
+     313             :   }
+     314             : 
+     315             :   // check age of input
+     316      330362 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     317          27 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     318             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     319          27 :     is_input_ready_ = false;
+     320             :   }
+     321             : 
+     322      330098 :   if (!isRunning() && !isStarted()) {
+     323        5894 :     return;
+     324             :   }
+     325             : 
+     326      324181 :   if (first_iter_) {
+     327         172 :     first_iter_ = false;
+     328         172 :     return;
+     329             :   }
+     330             : 
+     331      324009 :   double dt = (event.current_real - event.last_real).toSec();
+     332      324131 :   if (dt <= 0.0 || dt > 1.0) {
+     333       26464 :     return;
+     334             :   }
+     335             : 
+     336      297667 :   if (!is_repredictor_enabled_) {  // repredictor calculates dt on its own
+     337      297697 :     setDt(dt);
+     338             :   }
+     339             : 
+     340             :   // prediction step
+     341      297518 :   u_t       u;
+     342      297384 :   ros::Time input_stamp;
+     343      297446 :   if (is_input_ready_) {
+     344      179237 :     mrs_msgs::EstimatorInputConstPtr msg            = sh_control_input_.getMsg();
+     345      179376 :     const tf2::Vector3               des_acc_global = getAccGlobal(msg, 0);  // we don't care about heading
+     346      179540 :     input_stamp                                     = msg->header.stamp;
+     347      177872 :     if (input_coeff_ != default_input_coeff_){
+     348         134 :       setInputCoeff(default_input_coeff_);
+     349             :     }
+     350      177870 :     u(0) = des_acc_global.getZ();
+     351             :   } else {
+     352      117844 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     353      118279 :     input_stamp = ros::Time::now();
+     354      118388 :     if (input_coeff_ != 0){
+     355         199 :       setInputCoeff(0);
+     356             :     }
+     357      118388 :     u = u_t::Zero();
+     358             :   }
+     359             : 
+     360             :   // go through available corrections and apply them
+     361             :   /* for (auto correction : corrections_) { */
+     362             :   /*   auto res = correction->getProcessedCorrection(); */
+     363             :   /*   if (res) { */
+     364             :   /*     auto measurement_stamped = res.value(); */
+     365             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     366             :   /*   } */
+     367             :   /* } */
+     368             : 
+     369      295620 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     370      297199 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     371             : 
+     372             :   try {
+     373             :     // Apply the prediction step
+     374      593408 :     std::scoped_lock lock(mutex_lkf_);
+     375      296602 :     if (is_repredictor_enabled_) {
+     376           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
+     377           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     378             :     } else {
+     379      296602 :       sc = lkf_->predict(sc, u, Q, dt_);
+     380             :     }
+     381             :   }
+     382           0 :   catch (const std::exception &e) {
+     383             :     // In case of error, alert the user
+     384           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     385             :   }
+     386             : 
+     387      294346 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     388             : 
+     389             :   // publishing
+     390      295975 :   publishInput(u, input_stamp);
+     391      298018 :   publishOutput();
+     392      298038 :   publishDiagnostics();
+     393             : }
+     394             : /*//}*/
+     395             : 
+     396             : /*//{ timerCheckHealth() */
+     397           0 : void AltGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     398             : 
+     399           0 :   if (!isInitialized()) {
+     400           0 :     return;
+     401             :   }
+     402             : 
+     403           0 :   switch (getCurrentSmState()) {
+     404             : 
+     405           0 :     case UNINITIALIZED_STATE: {
+     406           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     407           0 :       break;
+     408             :     }
+     409             : 
+     410           0 :     case READY_STATE: {
+     411           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     412           0 :       break;
+     413             :     }
+     414             : 
+     415           0 :     case INITIALIZED_STATE: {
+     416             : 
+     417             :       // initialize the estimator with current corrections
+     418           0 :       for (auto correction : corrections_) {
+     419           0 :         auto res = correction->getProcessedCorrection();
+     420           0 :         if (res) {
+     421           0 :           auto measurement_stamped = res.value();
+     422           0 :           setState(measurement_stamped.value(0), correction->getStateId());
+     423           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     424             :         } else {
+     425           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     426             :                             correction->getNamespacedName().c_str());
+     427           0 :           return;
+     428             :         }
+     429             :       }
+     430           0 :       changeState(READY_STATE);
+     431           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     432           0 :       break;
+     433             :     }
+     434             : 
+     435           0 :     case STARTED_STATE: {
+     436           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     437             : 
+     438           0 :       if (isConverged()) {
+     439           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     440           0 :         changeState(RUNNING_STATE);
+     441             :       }
+     442           0 :       break;
+     443             :     }
+     444             : 
+     445           0 :     case RUNNING_STATE: {
+     446           0 :       for (auto correction : corrections_) {
+     447           0 :         if (!correction->isHealthy()) {
+     448           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     449           0 :           changeState(ERROR_STATE);
+     450             :         }
+     451             :       }
+     452           0 :       break;
+     453             :     }
+     454             : 
+     455           0 :     case STOPPED_STATE: {
+     456           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     457           0 :       break;
+     458             :     }
+     459             : 
+     460           0 :     case ERROR_STATE: {
+     461           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     462           0 :       bool all_corrections_healthy = true;
+     463           0 :       for (auto correction : corrections_) {
+     464           0 :         if (!correction->isHealthy()) {
+     465           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     466           0 :           all_corrections_healthy = false;
+     467             :         }
+     468             :       }
+     469             :       // initialize the estimator again if corrections become healthy
+     470           0 :       if (all_corrections_healthy) {
+     471           0 :         changeState(INITIALIZED_STATE);
+     472             :       }
+     473           0 :       break;
+     474             :     }
+     475             :   }
+     476             : 
+     477           0 :   if (sh_control_input_.newMsg()) {
+     478           0 :     is_input_ready_ = true;
+     479             :   }
+     480             : 
+     481             :   // check age of input
+     482           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     483           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     484             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     485           0 :     is_input_ready_ = false;
+     486             :   }
+     487             : }
+     488             : /*//}*/
+     489             : 
+     490             : /*//{ doCorrection() */
+     491      459373 : void AltGeneric::doCorrection(const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     492      459373 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     493      459073 : }
+     494             : /*//}*/
+     495             : 
+     496             : /*//{ doCorrection() */
+     497      459355 : void AltGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     498             : 
+     499      459355 :   if (!isInitialized()) {
+     500        4343 :     return;
+     501             :   }
+     502             : 
+     503             :   // we do not want to perform corrections until the estimator is initialized
+     504      459210 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     505        4340 :     return;
+     506             :   }
+     507             : 
+     508             :   // for position state check the innovation
+     509      454708 :   if (H_idx == POSITION) {
+     510      263347 :     std::scoped_lock lock(mtx_innovation_);
+     511             : 
+     512      263397 :     is_mitigating_jump_ = false;
+     513      263504 :     innovation_(0)      = z(0) - getState(POSITION);
+     514             : 
+     515      263461 :     if (fabs(innovation_(0)) > pos_innovation_limit_) {
+     516           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - [%.2f] lim: %.2f", getPrintName().c_str(), innovation_(0), pos_innovation_limit_);
+     517           0 :       innovation_ok_ = false;
+     518           0 :       switch (exc_innovation_action_) {
+     519           0 :         case ExcInnoAction_t::ELAND: {
+     520           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger eland in control manager", getPrintName().c_str());
+     521           0 :           changeState(ERROR_STATE);
+     522           0 :           break;
+     523             :         }
+     524           0 :         case ExcInnoAction_t::SWITCH: {
+     525           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: innovation should trigger estimator switch but no eland", getPrintName().c_str());
+     526           0 :           innovation_(0) = 0.0;  // this is quite hacky but is there other way to switch estimators and not trigger eland by the large innovation?
+     527           0 :           changeState(ERROR_STATE);
+     528           0 :           break;
+     529             :         }
+     530           0 :         case ExcInnoAction_t::MITIGATE: {
+     531           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger estimate jump mitigation", getPrintName().c_str());
+     532           0 :           innovation_(0)      = 0.0;  // this is quite hacky but is there other way to switch estimators and not trigger eland by the large innovation?
+     533           0 :           is_mitigating_jump_ = true;
+     534           0 :           setState(z(0), POSITION);
+     535           0 :           break;
+     536             :         }
+     537           0 :         case ExcInnoAction_t::NONE: {
+     538           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation ignored", getPrintName().c_str());
+     539           0 :           break;
+     540             :         }
+     541             :       }
+     542             :     }
+     543      263465 :     innovation_ok_ = true;
+     544             :   }
+     545             : 
+     546      454803 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     547             :   try {
+     548             :     // Apply the correction step
+     549             :     {
+     550      908341 :       std::scoped_lock lock(mutex_lkf_);
+     551      454980 :       H_        = H_t::Zero();
+     552      454713 :       H_(H_idx) = 1;
+     553      454299 :       lkf_->H   = H_;
+     554      453667 :       if (is_repredictor_enabled_) {
+     555             : 
+     556           0 :         lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     557             :       } else {
+     558      453667 :         sc = lkf_->correct(sc, z, R_t::Ones() * R);
+     559             :       }
+     560             :     }
+     561             :   }
+     562           0 :   catch (const std::exception &e) {
+     563             :     // In case of error, alert the user
+     564           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     565             :   }
+     566             : 
+     567      451321 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     568             : }  // namespace mrs_uav_state_estimators
+     569             : /*//}*/
+     570             : 
+     571             : /*//{ isConverged() */
+     572         172 : bool AltGeneric::isConverged() {
+     573             : 
+     574             :   // TODO: check convergence by rate of change of determinant
+     575             : 
+     576         172 :   return true;
+     577             : }
+     578             : /*//}*/
+     579             : 
+     580             : /*//{ getState() */
+     581      141810 : double AltGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     582      141810 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     583             : }
+     584             : 
+     585     1095205 : double AltGeneric::getState(const int &state_idx_in) const {
+     586     1095205 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     587             : }
+     588             : /*//}*/
+     589             : 
+     590             : /*//{ setState() */
+     591           0 : void AltGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     592           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     593           0 : }
+     594             : 
+     595         266 : void AltGeneric::setState(const double &state_in, const int &state_idx_in) {
+     596         266 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     597         266 : }
+     598             : /*//}*/
+     599             : 
+     600             : /*//{ getStates() */
+     601      297907 : AltGeneric::states_t AltGeneric::getStates(void) const {
+     602      595247 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     603             : }
+     604             : /*//}*/
+     605             : 
+     606             : /*//{ setStates() */
+     607           0 : void AltGeneric::setStates(const states_t &states_in) {
+     608           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     609           0 : }
+     610             : /*//}*/
+     611             : 
+     612             : /*//{ getCovariance() */
+     613           0 : double AltGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     614           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     615             : }
+     616             : 
+     617      645652 : double AltGeneric::getCovariance(const int &state_idx_in) const {
+     618      645652 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     619             : }
+     620             : /*//}*/
+     621             : 
+     622             : /*//{ getCovarianceMatrix() */
+     623      297728 : AltGeneric::covariance_t AltGeneric::getCovarianceMatrix(void) const {
+     624      594921 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     625             : }
+     626             : /*//}*/
+     627             : 
+     628             : /*//{ setCovarianceMatrix() */
+     629           0 : void AltGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     630           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     631           0 : }
+     632             : /*//}*/
+     633             : 
+     634             : /*//{ getInnovation() */
+     635      183559 : double AltGeneric::getInnovation(const int &state_idx) const {
+     636      183559 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     637             : }
+     638             : 
+     639           0 : double AltGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     640           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     641             : }
+     642             : /*//}*/
+     643             : 
+     644             : /*//{ setDt() */
+     645      297946 : void AltGeneric::setDt(const double &dt) {
+     646      297946 :   dt_ = dt;
+     647      297946 :   generateA();
+     648      297705 :   generateB();
+     649      595449 :   std::scoped_lock lock(mutex_lkf_);
+     650      297582 :   lkf_->A = A_;
+     651      297528 :   lkf_->B = B_;
+     652      297355 : }
+     653             : /*//}*/
+     654             : 
+     655             : /*//{ setInputCoeff() */
+     656         334 : void AltGeneric::setInputCoeff(const double &input_coeff) {
+     657         334 :   input_coeff_ = input_coeff;
+     658         334 :   generateA();
+     659         332 :   generateB();
+     660         664 :   std::scoped_lock lock(mutex_lkf_);
+     661         331 :   lkf_->A = A_;
+     662         334 :   lkf_->B = B_;
+     663             : 
+     664         334 :   if (is_repredictor_enabled_) {
+     665           0 :     models_.clear();
+     666           0 :     generateRepredictorModels(input_coeff_);
+     667             :   }
+     668         333 : }
+     669             : /*//}*/
+     670             : 
+     671             : /*//{ generateRepredictorModels() */
+     672           0 : void AltGeneric::generateRepredictorModels(const double input_coeff) {
+     673             : 
+     674           0 :     for (int i = 0; i < alt_generic::n_states; i++) {
+     675             : 
+     676           0 :       auto lambda_generateA = [input_coeff](const double dt) {
+     677           0 :         A_t A;
+     678             :         // clang-format off
+     679           0 :         A <<
+     680           0 :           1, dt, 0.5 * dt * dt,
+     681           0 :           0, 1, dt,
+     682           0 :           0, 0, 1-(input_coeff * dt);
+     683             :         // clang-format on
+     684           0 :         return A;
+     685           0 :       };
+     686             : 
+     687           0 :       auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
+     688           0 :         B_t B = B.Zero();
+     689             :         // clang-format off
+     690           0 :         B <<
+     691             :           0,
+     692           0 :           0,
+     693           0 :           (input_coeff * dt);
+     694             :         // clang-format on
+     695           0 :         return B;
+     696           0 :       };
+     697             : 
+     698           0 :       H_t H = H_t::Zero();
+     699           0 :       H(i)  = 1;
+     700           0 :       models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
+     701             :     }
+     702           0 : }
+     703             : /*//}*/
+     704             : 
+     705             : /*//{ generateA() */
+     706      298489 : void AltGeneric::generateA() {
+     707             : 
+     708             :   // clang-format off
+     709      298489 :     A_ <<
+     710      297924 :       1, dt_, 0.5 * dt_ * dt_,
+     711      298063 :       0, 1, dt_,
+     712      298128 :       0, 0, 1-(input_coeff_ * dt_);
+     713             :   // clang-format on
+     714      297957 : }
+     715             : /*//}*/
+     716             : 
+     717             : /*//{ generateB() */
+     718      298504 : void AltGeneric::generateB() {
+     719             : 
+     720             :   // clang-format off
+     721      298504 :     B_ <<
+     722             :       0,
+     723      297611 :       0,
+     724      297796 :       (input_coeff_ * dt_);
+     725             :   // clang-format on
+     726      297720 : }
+     727             : /*//}*/
+     728             : 
+     729             : /*//{ callbackReconfigure() */
+     730         172 : void AltGeneric::callbackReconfigure(AltitudeEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     731             : 
+     732         172 :   if (!isInitialized()) {
+     733         172 :     return;
+     734             :   }
+     735             : 
+     736           0 :   Q_t Q;
+     737           0 :   Q(POSITION, POSITION)         = config.pos;
+     738           0 :   Q(VELOCITY, VELOCITY)         = config.vel;
+     739           0 :   Q(ACCELERATION, ACCELERATION) = config.acc;
+     740           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     741             : }
+     742             : /*//}*/
+     743             : 
+     744             : /*//{ getNamespacedName() */
+     745        1111 : std::string AltGeneric::getNamespacedName() const {
+     746        2222 :   return parent_state_est_name_ + "/" + getName();
+     747             : }
+     748             : /*//}*/
+     749             : 
+     750             : /*//{ getPrintName() */
+     751        1345 : std::string AltGeneric::getPrintName() const {
+     752        2690 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     753             : }
+     754             : /*//}*/
+     755             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..e22283543d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html @@ -0,0 +1,209 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5c074cd2ef4a26539e9085a2f68e143e680c1d75 GIT binary patch literal 2919 zcmV-t3z+nYP)7#*!!SV$K00CmYSV z=-_y!Tv4g=EtIJy0+}j+(lgo3a{I;PPhb+FGj6!4xda>sq2IRxu-q~k*`C$@zwX=fKW2FBcl!ndY}lSFjT9G1vQl);SP)2ORGY$0|IAN?AnEBI z3qsKgNVcghW};B^N)MA7&eqh}A7RQIOo)Op0VH#y00vd4L=8Tu0EmI9o{IDQ86$~W zh@RK*)bhbD=?pj=#;1lpU+(Mb-GU0BQ{Hv0Xs%zQdrg4C=v(7+-Yxz6a-G+@Gf9Y0 zc%)vr(i1WphaaB$II3s((i-b7*AUE|OF7l7nY4066`1PjnFzw@0X?@WfH^l+K;tng zbC**$0S!uwLt5^*iGbi>Q`h9@#ZYRj3Pp#J0v70wI5g7CrZzgm5B^l5sN*zvb!v5H zW^r@#Q&_Y2*&cY5PMl0RMMx%oJB+}b2E>Dv{%XpjCDn>_7dyU|D#;&k?&Dls|IU7&pe1!gSjjavG zEd{J(jWhLy0gG9xLaVMbm}*(T1mn~A=@=VMSpvAWg1w$K!qO9_Zdn%G_Wbigp$de@ z7^n5No=pGF&e02Y1|Fc?_Q+#!uSfswu9P)n$h8KiLYtZck^`auhNE=T$$0`=GypNZ zR6oDpToRzh@GKs5*&%6-379ImzZ2x@fLM@wncxOTYEK-~m)NsImU$UKT9zx!9%Yv5 zfJF*^WSg(3aZ*)Qnce4$;ukRsjVuI3g`!emB&Q+DF8@Bc=LdW z;DXxK!Ja9wr@DhvmhhcJ6~_ljG&~@{#|hl*SBR<81FeOGB!eTvH5o0@^nK1dW za)FuVY+<#yu#C=3p$KOB5LIZIW?=)97*pKB2F7>>H2{mbO_a_!3cyN66J+Gq>~KX&BEnDe%aNCVxHU5o6EB^82yh zV+(R2@bREL=Xt^&N*g)sYi?4CqPV7og3|ni;TEozSWDT{p1DH{yS5OJvaVdp={Ed= z4z@Jx&>Ra^=c+fJUl52Nw#-_yMyiB}F+`RSS4l+@<6TI@?;~*6l{i%3;~3qqf0aFv z>N)gVYRz-+%%$jFXt~BF7vD{cxDiKMi^lUf99MIL8=KFUh&@%HtYCdARiFW24 zYmDWBRw-w4Ohm7oRQu~9AbejTw+Z1EO=>S3U^;V*>Eb3lp1TlLp`45`hN&|PAk!tc zUdqIP5z>@(`00HqX!k?1#3Q}LvxTvfF|ic2xl{I3PcCNU8*7BwQ*YuVz$0pB!oV+X5GN+^oQMnbt1)Jh>_u6~+NxypxeDdn2 z7^)0lOtBnkMKa??>&)p$4592^SX}FZGCdvUn6Y|jx!${E4FfAhdub5AUs6uZ4-cbs zlXu^DZF4heva4p;tKupFwv0X8Ja<(Ci)Itq*z-&QlY})93UG8bumGZVQF))yeNy+s zpDED(B##^O(P_i!ClFJorz@Gtx{#cz-Iyc6r|wC8TC*%DV+pvn53i7kBYa9vj+omE zTS(F<0Iu->37y8wQXuE6j4_-{MgB1kVX>a9h_T)3ww#^t%@)>W06f`A8u*eV;0rp_ zU6l%{<8@;etuOT{XTVFqV(y~(@yWk3Gh8Vw_S9z}z_;$qhm>7@v68!d$ldp$jbb z&H_`du$=tIE%VKuEZ;)Tn{GKD$?wmiS|%IZ!i?(0rBxR9w4X?> z=Rk&0_i0i-zL`CWbJk){#BsqI2izwG;i5I0>({z3dw3vLSVWxl@|;xA+jVCGzT^Fm)0EdLJrji5FuK<#Yr;QT(=Z_W+-a@ey^`W{1clc;WxTLxFuKDc$d;7o0Rq{klZj! z6rZB?0eOmM)0Ze*g)~;=u~_2=$YYagNxXu$kV?;<4)T{X>4D{OSEGXru$$^8-Gx@M zJ1J{?z3zD{hpL7+)$kCdEVSgZ)I!5P(t|`lK^~i%BNI_7<5y{6lWI}OR?@TJp^I7= z?`Cl^`vxtnNO)p&6)z>bo8Gn8@OY!8xJn-TA9{COJ(Bp?@hKI6OjkyYY= zAK=aa$|YDw;UZS{X)(1&jQpi7Y%Y#4g0U<9Fm(6{&|mW$P2iN%T7hFq-XC&ZO|do? zv#wiGWM1>cPv^zhSQICuN0hcEfOc{S;}Q?@Y9GL2Q*qcD9ZK8Ixm9om!0NOqpy!{G z4a + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
<unnamed>53.9 %213 / 39566.7 %22 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html new file mode 100644 index 0000000000..fb1e51241e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
<unnamed>53.9 %213 / 39566.7 %22 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-detail.html b/mrs_uav_state_estimators/src/estimators/altitude/index-detail.html new file mode 100644 index 0000000000..2a6a30a21f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
<unnamed>53.9 %213 / 39566.7 %22 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html new file mode 100644 index 0000000000..d98c1d5c4e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html new file mode 100644 index 0000000000..891f3b9266 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index.html b/mrs_uav_state_estimators/src/estimators/altitude/index.html new file mode 100644 index 0000000000..99f43bb63e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-25 05:42:57Functions:223366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..f481990f43 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html @@ -0,0 +1,216 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03570.0 %
Date:2024-04-25 05:42:57Functions:0340.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
mrs_uav_state_estimators::HdgGeneric::isConverged()0
mrs_uav_state_estimators::HdgGeneric::timerUpdate(ros::TimerEvent const&)0
mrs_uav_state_estimators::HdgGeneric::doCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double, mrs_uav_managers::estimation_manager::StateId_t const&, ros::Time const&)0
mrs_uav_state_estimators::HdgGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)0
mrs_uav_state_estimators::HdgGeneric::setInputCoeff(double const&)0
mrs_uav_state_estimators::HdgGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::HdgGeneric::callbackReconfigure(mrs_uav_state_estimators::HeadingEstimatorConfig&, unsigned int)0
mrs_uav_state_estimators::HdgGeneric::setCovarianceMatrix(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&)0
mrs_uav_state_estimators::HdgGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::HdgGeneric::pause()0
mrs_uav_state_estimators::HdgGeneric::reset()0
mrs_uav_state_estimators::HdgGeneric::setDt(double const&)0
mrs_uav_state_estimators::HdgGeneric::start()0
mrs_uav_state_estimators::HdgGeneric::setState(double const&, int const&)0
mrs_uav_state_estimators::HdgGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::HdgGeneric::generateA()0
mrs_uav_state_estimators::HdgGeneric::generateB()0
mrs_uav_state_estimators::HdgGeneric::setStates(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)0
mrs_uav_state_estimators::HdgGeneric::getPrintName[abi:cxx11]() const0
mrs_uav_state_estimators::HdgGeneric::getCovariance(int const&) const0
mrs_uav_state_estimators::HdgGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::HdgGeneric::getInnovation(int const&) const0
mrs_uav_state_estimators::HdgGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::HdgGeneric::getLastValidHdg() const0
mrs_uav_state_estimators::HdgGeneric::getNamespacedName[abi:cxx11]() const0
mrs_uav_state_estimators::HdgGeneric::getCovarianceMatrix() const0
mrs_uav_state_estimators::HdgGeneric::getState(int const&) const0
mrs_uav_state_estimators::HdgGeneric::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgGeneric::getStates() const0
mrs_uav_state_estimators::HdgGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t)#2}::operator()(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t) const0
mrs_uav_state_estimators::HdgGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(int, int)#1}::operator()(int, int) const0
mrs_uav_state_estimators::HdgGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::HdgGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html new file mode 100644 index 0000000000..7f0347922b --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html @@ -0,0 +1,216 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03570.0 %
Date:2024-04-25 05:42:57Functions:0340.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)0
mrs_uav_state_estimators::HdgGeneric::isConverged()0
mrs_uav_state_estimators::HdgGeneric::timerUpdate(ros::TimerEvent const&)0
mrs_uav_state_estimators::HdgGeneric::doCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, double, mrs_uav_managers::estimation_manager::StateId_t const&, ros::Time const&)0
mrs_uav_state_estimators::HdgGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)0
mrs_uav_state_estimators::HdgGeneric::setInputCoeff(double const&)0
mrs_uav_state_estimators::HdgGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::HdgGeneric::callbackReconfigure(mrs_uav_state_estimators::HeadingEstimatorConfig&, unsigned int)0
mrs_uav_state_estimators::HdgGeneric::setCovarianceMatrix(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&)0
mrs_uav_state_estimators::HdgGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::HdgGeneric::pause()0
mrs_uav_state_estimators::HdgGeneric::reset()0
mrs_uav_state_estimators::HdgGeneric::setDt(double const&)0
mrs_uav_state_estimators::HdgGeneric::start()0
mrs_uav_state_estimators::HdgGeneric::setState(double const&, int const&)0
mrs_uav_state_estimators::HdgGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::HdgGeneric::generateA()0
mrs_uav_state_estimators::HdgGeneric::generateB()0
mrs_uav_state_estimators::HdgGeneric::setStates(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)0
mrs_uav_state_estimators::HdgGeneric::getPrintName[abi:cxx11]() const0
mrs_uav_state_estimators::HdgGeneric::getCovariance(int const&) const0
mrs_uav_state_estimators::HdgGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::HdgGeneric::getInnovation(int const&) const0
mrs_uav_state_estimators::HdgGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::HdgGeneric::getLastValidHdg() const0
mrs_uav_state_estimators::HdgGeneric::getNamespacedName[abi:cxx11]() const0
mrs_uav_state_estimators::HdgGeneric::getCovarianceMatrix() const0
mrs_uav_state_estimators::HdgGeneric::getState(int const&) const0
mrs_uav_state_estimators::HdgGeneric::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgGeneric::getStates() const0
mrs_uav_state_estimators::HdgGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t)#2}::operator()(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t) const0
mrs_uav_state_estimators::HdgGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(int, int)#1}::operator()(int, int) const0
mrs_uav_state_estimators::HdgGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::HdgGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.frameset.html new file mode 100644 index 0000000000..3cdb79f846 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html new file mode 100644 index 0000000000..df0ea3b3c1 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html @@ -0,0 +1,795 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03570.0 %
Date:2024-04-25 05:42:57Functions:0340.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #define VERSION "0.0.6.0"
+       2             : 
+       3             : /* includes //{ */
+       4             : 
+       5             : #include <mrs_uav_state_estimators/estimators/heading/hdg_generic.h>
+       6             : 
+       7             : //}
+       8             : 
+       9             : namespace mrs_uav_state_estimators
+      10             : 
+      11             : {
+      12             : 
+      13             : /* initialize() //{*/
+      14           0 : void HdgGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16           0 :   ch_ = ch;
+      17           0 :   ph_ = ph;
+      18             : 
+      19           0 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // clang-format off
+      22           0 :   dt_ = 0.01;
+      23           0 :   input_coeff_ = 20;
+      24           0 :   default_input_coeff_ = 20;
+      25             : 
+      26           0 :   generateA();
+      27           0 :   generateB();
+      28             : 
+      29           0 :   H_ <<
+      30           0 :     1, 0;
+      31             : 
+      32           0 :   innovation_ << 
+      33             :     0;
+      34             : 
+      35             : 
+      36             :   // clang-format on
+      37             : 
+      38             :   // | --------------- initialize parameter loader -------------- |
+      39             : 
+      40           0 :   if (is_core_plugin_) {
+      41             : 
+      42           0 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      43           0 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      44             :   }
+      45             : 
+      46           0 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      47             : 
+      48             :   // | --------------------- load parameters -------------------- |
+      49           0 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      50           0 :   ph->param_loader->loadParam("position_innovation_limit", pos_innovation_limit_);
+      51           0 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      52           0 :   if (is_repredictor_enabled_) {
+      53           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      54             :   }
+      55             : 
+      56             :   // | --------------- corrections initialization --------------- |
+      57           0 :   ph->param_loader->loadParam("corrections", correction_names_);
+      58             : 
+      59           0 :   for (auto corr_name : correction_names_) {
+      60           0 :     corrections_.push_back(std::make_shared<Correction<hdg_generic::n_measurements>>(
+      61           0 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::HEADING, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      62           0 :         [this](const Correction<hdg_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      63           0 :           return this->doCorrection(meas, R, state);
+      64             :         }));
+      65             :   }
+      66             : 
+      67           0 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      68             : 
+      69             :   // | ----------- initialize process noise covariance ---------- |
+      70           0 :   Q_ = Q_t::Zero();
+      71             :   double tmp_noise;
+      72           0 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      73           0 :   Q_(POSITION, POSITION) = tmp_noise;
+      74           0 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      75           0 :   Q_(VELOCITY, VELOCITY) = tmp_noise;
+      76             : 
+      77             :   // | ------- check if all parameters loaded successfully ------ |
+      78           0 :   if (!ph->param_loader->loadedSuccessfully()) {
+      79           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      80           0 :     ros::shutdown();
+      81             :   }
+      82             : 
+      83             :   // | ------------- initialize dynamic reconfigure ------------- |
+      84             :   drmgr_ =
+      85           0 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&HdgGeneric::callbackReconfigure, this, _1, _2));
+      86           0 :   drmgr_->config.pos = Q_(POSITION, POSITION);
+      87           0 :   drmgr_->config.vel = Q_(VELOCITY, VELOCITY);
+      88           0 :   drmgr_->update_config(drmgr_->config);
+      89             : 
+      90             :   // | --------------- Kalman filter intialization -------------- |
+      91           0 :   const x_t        x0 = x_t::Zero();
+      92           0 :   const P_t        P0 = 1e3 * P_t::Identity();
+      93           0 :   const statecov_t sc0({x0, P0});
+      94           0 :   sc_ = sc0;
+      95             : 
+      96           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+      97           0 :   if (is_repredictor_enabled_) {
+      98             : 
+      99           0 :     generateRepredictorModels(input_coeff_);
+     100             : 
+     101           0 :     const u_t       u0 = u_t::Zero();
+     102           0 :     const ros::Time t0 = ros::Time::now();
+     103           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
+     104             : 
+     105           0 :     setDt(1.0 / ch_->desired_uav_state_rate);
+     106             :   }
+     107             : 
+     108             :   // | ------------------ timers initialization ----------------- |
+     109           0 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgGeneric::timerUpdate, this);  // not running after init
+     110             :   /* timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgGeneric::timerCheckHealth, this); */
+     111             : 
+     112             :   // | --------------- subscribers initialization --------------- |
+     113             :   // subscriber to odometry
+     114           0 :   mrs_lib::SubscribeHandlerOptions shopts;
+     115           0 :   shopts.nh                 = nh;
+     116           0 :   shopts.node_name          = getPrintName();
+     117           0 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     118           0 :   shopts.threadsafe         = true;
+     119           0 :   shopts.autostart          = true;
+     120           0 :   shopts.queue_size         = 10;
+     121           0 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     122             : 
+     123           0 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     124             : 
+     125             :   // | ---------------- publishers initialization --------------- |
+     126           0 :   if (ch_->debug_topics.input) {
+     127           0 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     128             :   }
+     129           0 :   if (ch_->debug_topics.output) {
+     130           0 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     131             :   }
+     132           0 :   if (ch_->debug_topics.diag) {
+     133           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+     134             :   }
+     135             : 
+     136             :   // | ------------------ finish initialization ----------------- |
+     137             : 
+     138           0 :   if (changeState(INITIALIZED_STATE)) {
+     139           0 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+     140             :   } else {
+     141           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+     142             :   }
+     143           0 : }
+     144             : /*//}*/
+     145             : 
+     146             : /*//{ start() */
+     147           0 : bool HdgGeneric::start(void) {
+     148             : 
+     149           0 :   if (isInState(READY_STATE)) {
+     150             :     /* timer_update_.start(); */
+     151           0 :     changeState(STARTED_STATE);
+     152           0 :     return true;
+     153             : 
+     154             :   } else {
+     155           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     156           0 :     return false;
+     157             :   }
+     158             : }
+     159             : /*//}*/
+     160             : 
+     161             : /*//{ pause() */
+     162           0 : bool HdgGeneric::pause(void) {
+     163             : 
+     164           0 :   if (isInState(RUNNING_STATE)) {
+     165           0 :     changeState(STOPPED_STATE);
+     166           0 :     return true;
+     167             : 
+     168             :   } else {
+     169           0 :     return false;
+     170             :   }
+     171             : }
+     172             : /*//}*/
+     173             : 
+     174             : /*//{ reset() */
+     175           0 : bool HdgGeneric::reset(void) {
+     176             : 
+     177           0 :   if (!isInitialized()) {
+     178           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     179           0 :     return false;
+     180             :   }
+     181             : 
+     182           0 :   changeState(STOPPED_STATE);
+     183             : 
+     184             :   // Initialize LKF state and covariance
+     185           0 :   const x_t        x0 = x_t::Zero();
+     186           0 :   const P_t        P0 = 1e6 * P_t::Identity();
+     187           0 :   const statecov_t sc0({x0, P0});
+     188           0 :   sc_ = sc0;
+     189             : 
+     190             :   // Instantiate the LKF itself
+     191           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     192           0 :   if (is_repredictor_enabled_) {
+     193             : 
+     194           0 :     const u_t       u0 = u_t::Zero();
+     195           0 :     const ros::Time t0 = ros::Time(0);
+     196           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
+     197             :   }
+     198             : 
+     199           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     200             : 
+     201           0 :   return true;
+     202             : }
+     203             : /*//}*/
+     204             : 
+     205             : /* timerUpdate() //{*/
+     206           0 : void HdgGeneric::timerUpdate(const ros::TimerEvent &event) {
+     207             : 
+     208           0 :   if (!isInitialized()) {
+     209           0 :     return;
+     210             :   }
+     211             : 
+     212           0 :   switch (getCurrentSmState()) {
+     213             : 
+     214           0 :     case UNINITIALIZED_STATE: {
+     215           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     216           0 :       break;
+     217             :     }
+     218             : 
+     219           0 :     case READY_STATE: {
+     220           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     221           0 :       break;
+     222             :     }
+     223             : 
+     224           0 :     case INITIALIZED_STATE: {
+     225             :       // initialize the estimator with current corrections
+     226           0 :       for (auto correction : corrections_) {
+     227           0 :         auto res = correction->getProcessedCorrection();
+     228           0 :         if (res) {
+     229           0 :           auto measurement_stamped = res.value();
+     230           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     231           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X));
+     232             :         } else {
+     233           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     234             :                             correction->getNamespacedName().c_str());
+     235           0 :           return;
+     236             :         }
+     237             :       }
+     238           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     239           0 :       changeState(READY_STATE);
+     240           0 :       break;
+     241             :     }
+     242             : 
+     243           0 :     case STARTED_STATE: {
+     244           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     245           0 :       if (isConverged()) {
+     246           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     247           0 :         changeState(RUNNING_STATE);
+     248             :       }
+     249           0 :       break;
+     250             :     }
+     251             : 
+     252           0 :     case RUNNING_STATE: {
+     253           0 :       for (auto correction : corrections_) {
+     254           0 :         if (!correction->isHealthy()) {
+     255           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     256           0 :           changeState(ERROR_STATE);
+     257             :         }
+     258             :       }
+     259           0 :       break;
+     260             :     }
+     261             : 
+     262           0 :     case STOPPED_STATE: {
+     263           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     264           0 :       break;
+     265             :     }
+     266             : 
+     267           0 :     case ERROR_STATE: {
+     268           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     269           0 :       bool all_corrections_healthy = true;
+     270           0 :       for (auto correction : corrections_) {
+     271           0 :         if (!correction->isHealthy()) {
+     272           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     273           0 :           all_corrections_healthy = false;
+     274             :         }
+     275             :       }
+     276             :       // initialize the estimator again if corrections become healthy
+     277           0 :       if (all_corrections_healthy && innovation_ok_) {
+     278           0 :         changeState(INITIALIZED_STATE);
+     279             :       }
+     280           0 :       break;
+     281             :     }
+     282             :   }
+     283             : 
+     284           0 :   if (sh_control_input_.newMsg()) {
+     285           0 :     is_input_ready_ = true;
+     286             :   }
+     287             : 
+     288             :   // check age of input
+     289           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     290           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     291             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     292           0 :     is_input_ready_ = false;
+     293             :   }
+     294             : 
+     295           0 :   if (!isRunning() && !isStarted()) {
+     296           0 :     return;
+     297             :   }
+     298             : 
+     299           0 :   if (first_iter_) {
+     300           0 :     first_iter_ = false;
+     301           0 :     return;
+     302             :   }
+     303             : 
+     304           0 :   double dt = (event.current_real - event.last_real).toSec();
+     305           0 :   if (dt <= 0.0 || dt > 1.0) {
+     306           0 :     return;
+     307             :   }
+     308             : 
+     309           0 :   if (!is_repredictor_enabled_) {  // repredictor calculates dt on its own
+     310           0 :     setDt(dt);
+     311             :   }
+     312             : 
+     313             :   // go through available corrections and apply them
+     314             :   /* for (auto correction : corrections_) { */
+     315             :   /*   auto res = correction->getProcessedCorrection(); */
+     316             :   /*   if (res) { */
+     317             :   /*     auto measurement_stamped = res.value(); */
+     318             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     319             :   /*   } */
+     320             :   /* } */
+     321             : 
+     322             :   // prediction step
+     323           0 :   u_t       u;
+     324           0 :   ros::Time input_stamp;
+     325           0 :   if (is_input_ready_) {
+     326           0 :     input_stamp = sh_control_input_.getMsg()->header.stamp;
+     327           0 :     if (input_coeff_ != default_input_coeff_){
+     328           0 :       setInputCoeff(default_input_coeff_);
+     329             :     }
+     330           0 :     u(0) = sh_control_input_.getMsg()->control_hdg_rate;
+     331             :   } else {
+     332           0 :     input_stamp = ros::Time::now();
+     333           0 :     if (input_coeff_ != 0){
+     334           0 :       setInputCoeff(0);
+     335             :     }
+     336           0 :     u = u_t::Zero();
+     337             :   }
+     338             : 
+     339           0 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     340           0 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     341             :   try {
+     342             :     // Apply the prediction step
+     343           0 :     std::scoped_lock lock(mutex_lkf_);
+     344           0 :     if (is_repredictor_enabled_) {
+     345           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
+     346           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     347             :     } else {
+     348           0 :       sc = lkf_->predict(sc, u, Q, dt_);
+     349             :     }
+     350             :   }
+     351           0 :   catch (const std::exception &e) {
+     352             :     // In case of error, alert the user
+     353           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     354             :   }
+     355             : 
+     356           0 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     357             : 
+     358           0 :   if (!isError()) {
+     359           0 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, sc.x(POSITION), last_valid_hdg_);
+     360             :   }
+     361             : 
+     362             :   // publishing
+     363           0 :   publishInput(u, input_stamp);
+     364           0 :   publishOutput();
+     365           0 :   publishDiagnostics();
+     366             : }
+     367             : /*//}*/
+     368             : 
+     369             : /*//{ timerCheckHealth() */
+     370           0 : void HdgGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     371             : 
+     372           0 :   if (!isInitialized()) {
+     373           0 :     return;
+     374             :   }
+     375             : 
+     376           0 :   switch (getCurrentSmState()) {
+     377             : 
+     378           0 :     case UNINITIALIZED_STATE: {
+     379           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     380           0 :       break;
+     381             :     }
+     382             : 
+     383           0 :     case READY_STATE: {
+     384           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     385           0 :       break;
+     386             :     }
+     387             : 
+     388           0 :     case INITIALIZED_STATE: {
+     389             :       // initialize the estimator with current corrections
+     390           0 :       for (auto correction : corrections_) {
+     391           0 :         auto res = correction->getProcessedCorrection();
+     392           0 :         if (res) {
+     393           0 :           auto measurement_stamped = res.value();
+     394           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     395           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X));
+     396             :         } else {
+     397           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     398             :                             correction->getNamespacedName().c_str());
+     399           0 :           return;
+     400             :         }
+     401             :       }
+     402           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     403           0 :       changeState(READY_STATE);
+     404           0 :       break;
+     405             :     }
+     406             : 
+     407           0 :     case STARTED_STATE: {
+     408           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     409           0 :       if (isConverged()) {
+     410           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     411           0 :         changeState(RUNNING_STATE);
+     412             :       }
+     413           0 :       break;
+     414             :     }
+     415             : 
+     416           0 :     case RUNNING_STATE: {
+     417           0 :       for (auto correction : corrections_) {
+     418           0 :         if (!correction->isHealthy()) {
+     419           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     420           0 :           changeState(ERROR_STATE);
+     421             :         }
+     422             :       }
+     423           0 :       break;
+     424             :     }
+     425             : 
+     426           0 :     case STOPPED_STATE: {
+     427           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     428           0 :       break;
+     429             :     }
+     430             : 
+     431           0 :     case ERROR_STATE: {
+     432           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     433           0 :       bool all_corrections_healthy = true;
+     434           0 :       for (auto correction : corrections_) {
+     435           0 :         if (!correction->isHealthy()) {
+     436           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     437           0 :           all_corrections_healthy = false;
+     438             :         }
+     439             :       }
+     440             :       // initialize the estimator again if corrections become healthy
+     441           0 :       if (all_corrections_healthy) {
+     442           0 :         changeState(INITIALIZED_STATE);
+     443             :       }
+     444           0 :       break;
+     445             :     }
+     446             :   }
+     447             : 
+     448           0 :   if (sh_control_input_.newMsg()) {
+     449           0 :     is_input_ready_ = true;
+     450             :   }
+     451             : 
+     452             :   // check age of input
+     453           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     454           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     455             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     456           0 :     is_input_ready_ = false;
+     457             :   }
+     458             : }
+     459             : /*//}*/
+     460             : 
+     461             : /*//{ doCorrection() */
+     462           0 : void HdgGeneric::doCorrection(const Correction<hdg_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     463           0 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     464           0 : }
+     465             : /*//}*/
+     466             : 
+     467             : /*//{ doCorrection() */
+     468           0 : void HdgGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     469             : 
+     470           0 :   if (!isInitialized()) {
+     471           0 :     return;
+     472             :   }
+     473             : 
+     474             :   // copy measurement as we might need to modify it (unwrap)
+     475           0 :   z_t meas = z;
+     476             : 
+     477             :   // we do not want to perform corrections until the estimator is initialized
+     478           0 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     479           0 :     return;
+     480             :   }
+     481             : 
+     482           0 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     483             : 
+     484             :   // for position state check the innovation
+     485           0 :   if (H_idx == POSITION) {
+     486             : 
+     487             :     // unwrap the hdg measurement wrt current state
+     488           0 :     meas(POSITION) = mrs_lib::geometry::radians::unwrap(meas(POSITION), sc.x(POSITION));
+     489             : 
+     490           0 :     std::scoped_lock lock(mtx_innovation_);
+     491             : 
+     492           0 :     innovation_(0) = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(meas(0)), mrs_lib::geometry::radians(sc.x(POSITION)));
+     493             : 
+     494           0 :     if (fabs(innovation_(0)) > pos_innovation_limit_) {
+     495           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - hdg: %.2f", getPrintName().c_str(), innovation_(0));
+     496           0 :       innovation_ok_ = false;
+     497           0 :       changeState(ERROR_STATE);
+     498             :     }
+     499             :   }
+     500             : 
+     501             :   try {
+     502             :     // Apply the correction step
+     503             :     {
+     504           0 :       std::scoped_lock lock(mutex_lkf_);
+     505           0 :       H_        = H_t::Zero();
+     506           0 :       H_(H_idx) = 1;
+     507           0 :       lkf_->H   = H_;
+     508           0 :       if (is_repredictor_enabled_) {
+     509           0 :         lkf_rep_->addMeasurement(meas, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     510             :       } else {
+     511           0 :         sc = lkf_->correct(sc, meas, R_t::Ones() * R);
+     512             :       }
+     513             :     }
+     514           0 :     innovation_ok_ = true;
+     515             :   }
+     516           0 :   catch (const std::exception &e) {
+     517             :     // In case of error, alert the user
+     518           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     519             :   }
+     520             : 
+     521           0 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     522             : }
+     523             : /*//}*/
+     524             : 
+     525             : /*//{ isConverged() */
+     526           0 : bool HdgGeneric::isConverged() {
+     527             : 
+     528             :   // TODO: check convergence by rate of change of determinant
+     529             : 
+     530           0 :   return true;
+     531             : }
+     532             : /*//}*/
+     533             : 
+     534             : /*//{ getState() */
+     535           0 : double HdgGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     536           0 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     537             : }
+     538             : 
+     539           0 : double HdgGeneric::getState(const int &state_idx_in) const {
+     540           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     541             : }
+     542             : /*//}*/
+     543             : 
+     544             : /*//{ setState() */
+     545           0 : void HdgGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     546           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     547           0 : }
+     548             : 
+     549           0 : void HdgGeneric::setState(const double &state_in, const int &state_idx_in) {
+     550           0 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     551           0 : }
+     552             : /*//}*/
+     553             : 
+     554             : /*//{ getStates() */
+     555           0 : HdgGeneric::states_t HdgGeneric::getStates(void) const {
+     556           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     557             : }
+     558             : /*//}*/
+     559             : 
+     560             : /*//{ setStates() */
+     561           0 : void HdgGeneric::setStates(const states_t &states_in) {
+     562           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     563           0 : }
+     564             : /*//}*/
+     565             : 
+     566             : /*//{ getCovariance() */
+     567           0 : double HdgGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     568           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     569             : }
+     570             : 
+     571           0 : double HdgGeneric::getCovariance(const int &state_idx_in) const {
+     572           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     573             : }
+     574             : /*//}*/
+     575             : 
+     576             : /*//{ getCovarianceMatrix() */
+     577           0 : HdgGeneric::covariance_t HdgGeneric::getCovarianceMatrix(void) const {
+     578           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     579             : }
+     580             : /*//}*/
+     581             : 
+     582             : /*//{ setCovarianceMatrix() */
+     583           0 : void HdgGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     584           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     585           0 : }
+     586             : /*//}*/
+     587             : 
+     588             : /*//{ getInnovation() */
+     589           0 : double HdgGeneric::getInnovation(const int &state_idx) const {
+     590           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     591             : }
+     592             : 
+     593           0 : double HdgGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     594           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     595             : }
+     596             : /*//}*/
+     597             : 
+     598             : /*//{ setDt() */
+     599           0 : void HdgGeneric::setDt(const double &dt) {
+     600           0 :   dt_ = dt;
+     601           0 :   generateA();
+     602           0 :   generateB();
+     603           0 :   std::scoped_lock lock(mutex_lkf_);
+     604           0 :   lkf_->A = A_;
+     605           0 :   lkf_->B = B_;
+     606           0 : }
+     607             : /*//}*/
+     608             : 
+     609             : /*//{ setInputCoeff() */
+     610           0 : void HdgGeneric::setInputCoeff(const double &input_coeff) {
+     611           0 :   input_coeff_ = input_coeff;
+     612           0 :   generateA();
+     613           0 :   generateB();
+     614           0 :   std::scoped_lock lock(mutex_lkf_);
+     615           0 :   lkf_->A = A_;
+     616           0 :   lkf_->B = B_;
+     617             : 
+     618           0 :   if (is_repredictor_enabled_) {
+     619           0 :     models_.clear();
+     620           0 :     generateRepredictorModels(input_coeff_);
+     621             :   }
+     622           0 : }
+     623             : /*//}*/
+     624             : 
+     625             : /*//{ generateRepredictorModels() */
+     626           0 : void HdgGeneric::generateRepredictorModels(const double input_coeff) {
+     627             : 
+     628           0 :     for (int i = 0; i < hdg_generic::n_states; i++) {
+     629             : 
+     630           0 :       auto lambda_generateA = [input_coeff](const double dt) {
+     631           0 :         A_t A;
+     632             :         // clang-format off
+     633           0 :         A <<
+     634           0 :           1, dt,
+     635           0 :           0, 1-(input_coeff * dt);
+     636             :         // clang-format on
+     637           0 :         return A;
+     638           0 :       };
+     639             : 
+     640           0 :       auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
+     641           0 :         B_t B = B.Zero();
+     642             :         // clang-format off
+     643           0 :         B <<
+     644             :           0,
+     645           0 :           (input_coeff * dt);
+     646             :         // clang-format on
+     647           0 :         return B;
+     648           0 :       };
+     649             : 
+     650           0 :       H_t H = H_t::Zero();
+     651           0 :       H(i)  = 1;
+     652           0 :       models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
+     653             :     }
+     654           0 : }
+     655             : /*//}*/
+     656             : 
+     657             : /*//{ generateA() */
+     658           0 : void HdgGeneric::generateA() {
+     659             : 
+     660             :   // clang-format off
+     661           0 :     A_ <<
+     662           0 :       1, dt_,
+     663           0 :       0, 1-(input_coeff_ * dt_);
+     664             :   // clang-format on
+     665           0 : }
+     666             : /*//}*/
+     667             : 
+     668             : /*//{ generateB() */
+     669           0 : void HdgGeneric::generateB() {
+     670             : 
+     671             :   // clang-format off
+     672           0 :     B_ <<
+     673             :       0,
+     674           0 :       (input_coeff_ * dt_);
+     675             :   // clang-format on
+     676           0 : }
+     677             : /*//}*/
+     678             : 
+     679             : /*//{ getLastValidHdg() */
+     680           0 : double HdgGeneric::getLastValidHdg() const {
+     681           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     682             : }
+     683             : /*//}*/
+     684             : 
+     685             : /*//{ callbackReconfigure() */
+     686           0 : void HdgGeneric::callbackReconfigure(HeadingEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     687             : 
+     688           0 :   if (!isInitialized()) {
+     689           0 :     return;
+     690             :   }
+     691             : 
+     692           0 :   Q_t Q;
+     693           0 :   Q(POSITION, POSITION) = config.pos;
+     694           0 :   Q(VELOCITY, VELOCITY) = config.vel;
+     695           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     696             : }
+     697             : /*//}*/
+     698             : 
+     699             : /*//{ getNamespacedName() */
+     700           0 : std::string HdgGeneric::getNamespacedName() const {
+     701           0 :   return parent_state_est_name_ + "/" + getName();
+     702             : }
+     703             : /*//}*/
+     704             : 
+     705             : /*//{ getPrintName() */
+     706           0 : std::string HdgGeneric::getPrintName() const {
+     707           0 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     708             : }
+     709             : /*//}*/
+     710             : 
+     711             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..74cfc37a09 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html @@ -0,0 +1,198 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..64f48b05578bee38911c93244748b9d1c7237750 GIT binary patch literal 2310 zcmV+h3HkPkP)%-Vg0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpRGaN<3|zBMI@TE#PU-5u=>d+Ns*?Q)3)Y7#U^< za!hgbl>^pVqU$;xb4&>cRCc7tofuiptm2xK!Z6lUHdm!sd93MpiVd&RA!d@Fl~ zpE}K2G?iqGhM_VdR6pyU`r7~WGZ$ta&Ya+x7U3D$m>r9PL>W*CSM(xN-$V^S7|4Gb#!04zp`=4&Bg1sW+nIVP3r^J-9 zY3?Kt!o>(+iQOy>%?X#h2|gh#gSR6%nutr1i8iM&yHvY(Y+A{a-N%0RLQcG zo3%nVF!3o@G0Mn3AIUh*dzKW%e3fPtq&Uf0&s2_(5ARDI!uyo9Cv2@L61vDyn7f}^ zC3L7Vo@wuKmkGf^rXJzv!$ZY71jS@#uL$UOE_4+(Q#;wkcFrWR)=5m)uh zb+HE?eQw5UPPEQ{gdkw6!-rdT^bZgJB1abwA6tDQ!R-jrnPo%DaAw9cDHBu)Yu#i+ zP1Vg{;1Wu?+STS1%2Kd;E;SLPRyb>>W-K7G2}3rVzq5>*!*Q6wi%i*5KNz=xnI4+& zkDdnvytAkteH^I_bVGrtGp8^V6KOD#i zPQ5ErI1UrdE;&p8uQ@AtLTGBtJ`?82$_zayD`U6zm~sbY#`<^}W8Wwz%vq0?FmKcO zvINi3PtL`>p3v%N!_<}PYGV8ijegb!WdUZ~aUp&MQ^Y2+fsU^{$JCIk+NsrGWvw~t zd5^*Hv*QAbxSVlZhi}&|g<)oYhozu}W+6yfw>24Q3HKvpkiKhS*(JEXb#4rlYwm>T zm6-1^I37}f!)CV%(>mT=nM|M8H3Hq!78k(rUQ1!C_Bpx~E>~uLzfOp#a^pZ)9f&?8oe(YiuqUj;OT*p?7g*}dNP=(OR~U$UPG7+v z*VVhgMVaDdF7@Nevo3CjE+t6?;bBxDmpMvn;5_qWt!*k>4T=Sq9rxtG)XS(^@oHsu zTNfDAh^VeL+7K~4^{}FOy=RgxmL`S1U*Yd=C(m3Vl(kLEr(;emPljKQd*89a|Gt62KQ^z*N|Dp({1UZ^xVF6v)7AHV0|JyWrYYLWshUj#J1uRxE&+rGZUQ?gPlaTd!6Q?= z!T{ND4DD@!@aP=Zkduke-$qNpMT4ZPyAomcOe&W(C9{|op8k}E6S`vaT>uTf!X3@> zm`3`>S(!c3dNH*ygW6t_RyU7`{+jU09umnXhC6)MLn3IU?WCuNq{#U6ke(hA!|~}M zJw2pO#_js0@br*4!l#EcHu%#+dU{Ab$D0868+u4mkbHVbL-;;DqzdFR$HRI^mtXr< zPp-__Rd{ZxE|FOJpiiUqyd%`^NU1-YF0EQL_hFZf@FJZe~X>X5v z@mG$I@(p$uJp)e7l;VF|b=74|=)pPCA{Q03^EH{k=5)?)M7CsLgmCAk)KaK%2ZWa; z9}E6iDcGNZg)|I&L_6*uw60j<=(KlzMM5`t=$jCJn!3hfL3JU(qX+5@#j}DX+!DjB zBSg1h*fRv+LmAP~{Qqn=-J$oP-MrW!e9!KM`5KAO3`mmlk)B`iz*2heg2s1iES_J3 zlq=2tlWY4knS39gX$=oi#-={c%1HkGZJueA&NY|XN%R#=z2@}p46HRsXCN7O4>3V1 zgJ+{+-r=~X-VS5I)uC~K$4TH)`O|0nm@-krm?4l zW*Pp2_()}R#63!UOv~{ecvPLkD?z_y$69yFe23P&uE~EX0I{pkZT&*}_ElH~U1=58 zjo~S+;W3c$>_~AI9_Qcl5e_|^j1%u+_v3g%fn?1(iZ{B&bXqJF1L{9SG~HBD-x1KQkOjGoC&@&YWIN)KD;6lKXZA*h4`+Rsy;|B3@@*sZ1tJUydD>L!yP^4NpN0=c# zcu^z*Z}YhDP!~c|o3skmi$GuGLe+~~m!`DkzpV8jp&`NW`s3cGa g<3>XtdgrG24_bz-Sm?6hp8x;=07*qoM6N<$f*@sYQ~&?~ literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html new file mode 100644 index 0000000000..5b75f43f52 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-04-25 05:42:57Functions:122352.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgPassthrough::setCovarianceMatrix(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&)0
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::HdgPassthrough::setStates(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)0
mrs_uav_state_estimators::HdgPassthrough::getCovariance(int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getInnovation(int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getLastValidHdg() const0
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::HdgPassthrough::start()167
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const198
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const564
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const190542
mrs_uav_state_estimators::HdgPassthrough::getStates() const190568
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)190581
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)193764
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)194325
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)198978
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)393270
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const665629
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html new file mode 100644 index 0000000000..3078640479 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-04-25 05:42:57Functions:122352.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::HdgPassthrough::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)190581
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)193764
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)198978
mrs_uav_state_estimators::HdgPassthrough::setCovarianceMatrix(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&)0
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)194325
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::start()167
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)393270
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::HdgPassthrough::setStates(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)0
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const564
mrs_uav_state_estimators::HdgPassthrough::getCovariance(int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getInnovation(int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getLastValidHdg() const0
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const198
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const190542
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const665629
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getStates() const190568
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.frameset.html new file mode 100644 index 0000000000..e033217e8c --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html new file mode 100644 index 0000000000..fbe06f2e9d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html @@ -0,0 +1,391 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-04-25 05:42:57Functions:122352.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #define VERSION "0.0.6.0"
+       2             : 
+       3             : /* includes //{ */
+       4             : 
+       5             : #include <mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h>
+       6             : 
+       7             : //}
+       8             : 
+       9             : namespace mrs_uav_state_estimators
+      10             : 
+      11             : {
+      12             : 
+      13             : /* initialize() //{*/
+      14          99 : void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          99 :   ch_ = ch;
+      17          99 :   ph_ = ph;
+      18             : 
+      19          99 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21          99 :   hdg_state_      = states_t::Zero();
+      22          99 :   hdg_covariance_ = covariance_t::Zero();
+      23             : 
+      24          99 :   innovation_ << 
+      25          99 :     0, 0;
+      26             : 
+      27             : 
+      28             :   // | --------------- param loader initialization --------------- |
+      29             : 
+      30          99 :   if (is_core_plugin_) {
+      31             : 
+      32          99 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      33          99 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      34             :   }
+      35             : 
+      36          99 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      37             : 
+      38             :   // | --------------------- load parameters -------------------- |
+      39          99 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      40             : 
+      41          99 :   ph->param_loader->loadParam("topics/orientation", orient_topic_);
+      42          99 :   ph->param_loader->loadParam("topics/angular_velocity", ang_vel_topic_);
+      43             : 
+      44          99 :   if (!ph->param_loader->loadedSuccessfully()) {
+      45           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      46           0 :     ros::shutdown();
+      47             :   }
+      48             : 
+      49             :   // | ------------------ timers initialization ----------------- |
+      50          99 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerUpdate, this, false, false);  // not running after init
+      51          99 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerCheckHealth, this);
+      52             : 
+      53             :   // | --------------- subscribers initialization --------------- |
+      54             :   // subscriber to odometry
+      55         198 :   mrs_lib::SubscribeHandlerOptions shopts;
+      56          99 :   shopts.nh                 = nh;
+      57          99 :   shopts.node_name          = getPrintName();
+      58          99 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      59          99 :   shopts.threadsafe         = true;
+      60          99 :   shopts.autostart          = true;
+      61          99 :   shopts.queue_size         = 10;
+      62          99 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      63             : 
+      64         198 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orient_topic_,
+      65          99 :                                                                                 &HdgPassthrough::callbackOrientation, this);
+      66         198 :   sh_ang_vel_     = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, "/" + ch_->uav_name + "/" + ang_vel_topic_,
+      67          99 :                                                                          &HdgPassthrough::callbackAngularVelocity, this);
+      68             : 
+      69             :   // | ---------------- publishers initialization --------------- |
+      70          99 :   if (ch_->debug_topics.output) {
+      71          99 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+      72             :   }
+      73          99 :   if (ch_->debug_topics.diag) {
+      74           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+      75             :   }
+      76             : 
+      77             :   // | ------------------ finish initialization ----------------- |
+      78             : 
+      79          99 :   if (changeState(INITIALIZED_STATE)) {
+      80          99 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+      81             :   } else {
+      82           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+      83             :   }
+      84          99 : }
+      85             : /*//}*/
+      86             : 
+      87             : /*//{ start() */
+      88         167 : bool HdgPassthrough::start(void) {
+      89             : 
+      90         167 :   if (isInState(READY_STATE)) {
+      91          99 :     timer_update_.start();
+      92          99 :     changeState(STARTED_STATE);
+      93          99 :     return true;
+      94             : 
+      95             :   } else {
+      96          68 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+      97          68 :     return false;
+      98             :   }
+      99             : }
+     100             : /*//}*/
+     101             : 
+     102             : /*//{ pause() */
+     103           0 : bool HdgPassthrough::pause(void) {
+     104             : 
+     105           0 :   if (isInState(RUNNING_STATE)) {
+     106           0 :     changeState(STOPPED_STATE);
+     107           0 :     return true;
+     108             : 
+     109             :   } else {
+     110           0 :     return false;
+     111             :   }
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ reset() */
+     116           0 : bool HdgPassthrough::reset(void) {
+     117             : 
+     118           0 :   if (!isInitialized()) {
+     119           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     120           0 :     return false;
+     121             :   }
+     122             : 
+     123           0 :   changeState(STOPPED_STATE);
+     124             : 
+     125           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     126             : 
+     127           0 :   return true;
+     128             : }
+     129             : /*//}*/
+     130             : 
+     131             : /*//{ callbackOrientation() */
+     132      198978 : void HdgPassthrough::callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     133             : 
+     134      198978 :   if (!isInitialized()) {
+     135          12 :     return;
+     136             :   }
+     137             : 
+     138             :   double hdg;
+     139             :   try {
+     140      198955 :     hdg = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     141             :   }
+     142           0 :   catch (...) {
+     143           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     144             :   }
+     145             : 
+     146      198929 :   setState(hdg, POSITION);
+     147             : 
+     148      198921 :   if (!isError()) {
+     149      198885 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, hdg, last_valid_hdg_);
+     150             :   }
+     151             : }
+     152             : /*//}*/
+     153             : 
+     154             : /*//{ callbackAngularVelocity() */
+     155      194325 : void HdgPassthrough::callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+     156             : 
+     157      194325 :   if (!isInitialized() || !sh_orientation_.hasMsg()) {
+     158           1 :     return;
+     159             :   }
+     160             : 
+     161             :   double hdg_rate;
+     162             :   try {
+     163      194318 :     hdg_rate = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+     164             :   }
+     165           0 :   catch (...) {
+     166           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     167             :   }
+     168             : 
+     169      194093 :   setState(hdg_rate, VELOCITY);
+     170             : }
+     171             : /*//}*/
+     172             : 
+     173             : /* timerUpdate() //{*/
+     174      190581 : void HdgPassthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     175             : 
+     176             : 
+     177      190581 :   if (!isInitialized()) {
+     178           0 :     return;
+     179             :   }
+     180             : 
+     181      190563 :   publishOutput();
+     182      190587 :   publishDiagnostics();
+     183             : }
+     184             : /*//}*/
+     185             : 
+     186             : /*//{ timerCheckHealth() */
+     187      193764 : void HdgPassthrough::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     188             : 
+     189      193764 :   if (!isInitialized()) {
+     190           3 :     return;
+     191             :   }
+     192             : 
+     193      193762 :   if (isInState(INITIALIZED_STATE) && is_orient_ready_ && is_ang_vel_ready_) {
+     194             : 
+     195          99 :     changeState(READY_STATE);
+     196          99 :     ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     197             :   }
+     198             : 
+     199      193762 :   if (isInState(STARTED_STATE)) {
+     200             : 
+     201          99 :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator Running", getPrintName().c_str());
+     202          99 :     changeState(RUNNING_STATE);
+     203             :   }
+     204             : 
+     205      193762 :   if (sh_orientation_.hasMsg()) {
+     206      192558 :     is_orient_ready_ = true;
+     207             :   } else {
+     208        1204 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation yet", getPrintName().c_str());
+     209             :   }
+     210             : 
+     211      193762 :   if (sh_ang_vel_.hasMsg()) {
+     212      190889 :     is_ang_vel_ready_ = true;
+     213             :   } else {
+     214        2873 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity yet", getPrintName().c_str());
+     215             :   }
+     216             : }
+     217             : /*//}*/
+     218             : 
+     219             : /*//{ getState() */
+     220           0 : double HdgPassthrough::getState(const int &state_id_in, const int &axis_in) const {
+     221           0 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     222             : }
+     223             : 
+     224      665629 : double HdgPassthrough::getState(const int &state_idx_in) const {
+     225      665629 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     226             : }
+     227             : /*//}*/
+     228             : 
+     229             : /*//{ setState() */
+     230           0 : void HdgPassthrough::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     231           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     232           0 : }
+     233             : 
+     234      393270 : void HdgPassthrough::setState(const double &state_in, const int &state_idx_in) {
+     235             : 
+     236      393270 :   const double prev_hdg_state   = mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     237      393029 :   prev_hdg_state_(state_idx_in) = prev_hdg_state;
+     238      393080 :   mrs_lib::set_mutexed(mtx_hdg_state_, state_in, hdg_state_(state_idx_in));
+     239             : 
+     240      393051 :   const double innovation = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(state_in), mrs_lib::geometry::radians(prev_hdg_state));
+     241      392984 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_(state_idx_in));
+     242      393101 : }
+     243             : /*//}*/
+     244             : 
+     245             : /*//{ getStates() */
+     246      190568 : HdgPassthrough::states_t HdgPassthrough::getStates(void) const {
+     247      190568 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_);
+     248             : }
+     249             : /*//}*/
+     250             : 
+     251             : /*//{ setStates() */
+     252           0 : void HdgPassthrough::setStates(const states_t &states_in) {
+     253           0 :   mrs_lib::set_mutexed(mtx_hdg_state_, states_in, hdg_state_);
+     254           0 : }
+     255             : /*//}*/
+     256             : 
+     257             : /*//{ getCovariance() */
+     258           0 : double HdgPassthrough::getCovariance(const int &state_id_in, const int &axis_in) const {
+     259           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     260             : }
+     261             : 
+     262           0 : double HdgPassthrough::getCovariance(const int &state_idx_in) const {
+     263           0 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_(state_idx_in, state_idx_in));
+     264             : }
+     265             : /*//}*/
+     266             : 
+     267             : /*//{ getCovarianceMatrix() */
+     268      190542 : HdgPassthrough::covariance_t HdgPassthrough::getCovarianceMatrix(void) const {
+     269      190542 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_);
+     270             : }
+     271             : /*//}*/
+     272             : 
+     273             : /*//{ setCovarianceMatrix() */
+     274           0 : void HdgPassthrough::setCovarianceMatrix(const covariance_t &cov_in) {
+     275           0 :   mrs_lib::set_mutexed(mtx_hdg_covariance_, cov_in, hdg_covariance_);
+     276           0 : }
+     277             : /*//}*/
+     278             : 
+     279             : /*//{ getInnovation() */
+     280           0 : double HdgPassthrough::getInnovation(const int &state_idx) const {
+     281           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_(state_idx));
+     282             : }
+     283             : 
+     284           0 : double HdgPassthrough::getInnovation(const int &state_id_in, const int &axis_in) const {
+     285           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     286             : }
+     287             : /*//}*/
+     288             : 
+     289             : /*//{ getLastValidHdg() */
+     290           0 : double HdgPassthrough::getLastValidHdg() const {
+     291           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     292             : }
+     293             : /*//}*/
+     294             : 
+     295             : /*//{ getNamespacedName() */
+     296         198 : std::string HdgPassthrough::getNamespacedName() const {
+     297         396 :   return parent_state_est_name_ + "/" + getName();
+     298             : }
+     299             : /*//}*/
+     300             : 
+     301             : /*//{ getPrintName() */
+     302         564 : std::string HdgPassthrough::getPrintName() const {
+     303        1128 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     304             : }
+     305             : /*//}*/
+     306             : 
+     307             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html new file mode 100644 index 0000000000..519a26d886 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html @@ -0,0 +1,97 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6c564f7e1e2332a7d663888dab973ff6736176bd GIT binary patch literal 1198 zcmV;f1X25mP)xhA{a0y>Im96${OsrCSN-)*s3M_Th;&+(W%hrUdAb6Y zm!mf?mr!#QKd=+!V36W9q859c&u^#!mv3?MN}RivHb$EUfLllZG$|oW$^aO+6j9FF zf!6W&@v>wxO7ZgwqGnjuyGVvZ9c2xskEo0msBwVxh@XXMeYu}Gy*qx7I<87ir>_x4 zDboYzB?E{OZ4c>@;Jn8N;DrGbCd)W;%$Zp}hMHN{foqamrt&J*BF%)s=Bvm1il2 zVxH|g6g5jMAj;r#=n{6P*&cOrOATHf8r=K1$DYCEnL-?7otk{(B~Z$GqTXy z5p#-`3QE+Q9f&l=^@#L7Yv~@v-V<2&GK)O-{P#{lI)tZ)I(xf?w%($rPxhBvMhrr~ z9a2wT;kK+}I7?9wGV3JN&U%|a#x{jUB&J-|;;C9l7eW;(muSj$`BY&#PW}=^VLzu- z=a>+W_EKTiBS^bWlqY)|!#jEWa1$qnaK09_GeW6@_pq*`=;0M0-}Q(;(^CinmI&8s z`Bb>-+<|q0_;k1~g8PJla5bHytJmWn4AyClrlNW>H7555WV5n3;_I0 zD*OEXQgegKGg50jXUi(%$aLmOsiP$9ct}1itknaDVsuzJUfeA;6NhMzKwm0cC$%=Q z;QUmh$tKYqwsKIB?g4FlZTBvv#*BSTR=LEdVKJC0+d~XgjjTJV8ncXP7a-qI;slnd z4C9wSL!KdAXap{*7)7{cEBs)XC?Ds=)6tozXjcOz7eAj!7*Bc8~XFid+4WH%&_xWRSE+QtGt&O;Yn>H(D3J zlAqMd+>^e + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9149018.6 %
Date:2024-04-25 05:42:57Functions:125721.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3570.0 %0 / 34
hdg_passthrough.cpp +
68.4%68.4%
+ 		68.4 %91 / 13352.2 %12 / 23
<unnamed>68.4 %91 / 13352.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html new file mode 100644 index 0000000000..19069ccd91 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9149018.6 %
Date:2024-04-25 05:42:57Functions:125721.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3570.0 %0 / 34
hdg_passthrough.cpp +
68.4%68.4%
+ 		68.4 %91 / 13352.2 %12 / 23
<unnamed>68.4 %91 / 13352.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-detail.html b/mrs_uav_state_estimators/src/estimators/heading/index-detail.html new file mode 100644 index 0000000000..f1160d9a49 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-detail.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9149018.6 %
Date:2024-04-25 05:42:57Functions:125721.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3570.0 %0 / 34
hdg_passthrough.cpp +
68.4%68.4%
+ 		68.4 %91 / 13352.2 %12 / 23
<unnamed>68.4 %91 / 13352.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html new file mode 100644 index 0000000000..a4dd0d2d73 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9149018.6 %
Date:2024-04-25 05:42:57Functions:125721.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3570.0 %0 / 34
hdg_passthrough.cpp +
68.4%68.4%
+ 		68.4 %91 / 13352.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html new file mode 100644 index 0000000000..bc81c9bebe --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9149018.6 %
Date:2024-04-25 05:42:57Functions:125721.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3570.0 %0 / 34
hdg_passthrough.cpp +
68.4%68.4%
+ 		68.4 %91 / 13352.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index.html b/mrs_uav_state_estimators/src/estimators/heading/index.html new file mode 100644 index 0000000000..5fd9abe1e7 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9149018.6 %
Date:2024-04-25 05:42:57Functions:125721.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3570.0 %0 / 34
hdg_passthrough.cpp +
68.4%68.4%
+ 		68.4 %91 / 13352.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html new file mode 100644 index 0000000000..b71f1c5d17 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
<unnamed>55.1 %241 / 43775.8 %25 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html new file mode 100644 index 0000000000..682af2c695 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
<unnamed>55.1 %241 / 43775.8 %25 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-detail.html b/mrs_uav_state_estimators/src/estimators/lateral/index-detail.html new file mode 100644 index 0000000000..cf100726af --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
<unnamed>55.1 %241 / 43775.8 %25 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html new file mode 100644 index 0000000000..dc450192c6 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html new file mode 100644 index 0000000000..79f2f0caa7 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index.html b/mrs_uav_state_estimators/src/estimators/lateral/index.html new file mode 100644 index 0000000000..749b952823 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..66978ae239 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::setCovarianceMatrix(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&)0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::LatGeneric::pause()0
mrs_uav_state_estimators::LatGeneric::reset()0
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::LatGeneric::isConverged()99
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)99
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)192
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)348
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)348
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const598
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const931
mrs_uav_state_estimators::LatGeneric::start()3930
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(int, int)#1}::operator()(int, int) const7646
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const172069
mrs_uav_state_estimators::LatGeneric::setDt(double const&)172070
mrs_uav_state_estimators::LatGeneric::getStates() const172070
mrs_uav_state_estimators::LatGeneric::generateA()172353
mrs_uav_state_estimators::LatGeneric::generateB()172360
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)193503
mrs_uav_state_estimators::LatGeneric::doCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, double, mrs_uav_managers::estimation_manager::StateId_t const&, ros::Time const&)198520
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t)#2}::operator()(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t) const198584
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)198606
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const367108
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const367112
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const734220
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const734223
mrs_uav_state_estimators::LatGeneric::getState(int const&) const1484067
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const1484208
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html new file mode 100644 index 0000000000..52f7e2a8a9 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::LatGeneric::isConverged()99
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)193503
mrs_uav_state_estimators::LatGeneric::doCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, double, mrs_uav_managers::estimation_manager::StateId_t const&, ros::Time const&)198520
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)198606
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)192
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)99
mrs_uav_state_estimators::LatGeneric::setCovarianceMatrix(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&)0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::LatGeneric::pause()0
mrs_uav_state_estimators::LatGeneric::reset()0
mrs_uav_state_estimators::LatGeneric::setDt(double const&)172070
mrs_uav_state_estimators::LatGeneric::start()3930
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)348
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)348
mrs_uav_state_estimators::LatGeneric::generateA()172353
mrs_uav_state_estimators::LatGeneric::generateB()172360
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const931
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const734220
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const734223
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const367108
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const367112
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const598
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const172069
mrs_uav_state_estimators::LatGeneric::getState(int const&) const1484067
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const1484208
mrs_uav_state_estimators::LatGeneric::getStates() const172070
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t)#2}::operator()(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t) const198584
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda(int, int)#1}::operator()(int, int) const7646
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html new file mode 100644 index 0000000000..20cba12156 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html new file mode 100644 index 0000000000..4fbf1f1966 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html @@ -0,0 +1,887 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-04-25 05:42:57Functions:253375.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #define VERSION "0.0.0.1"
+       2             : 
+       3             : /* includes //{ */
+       4             : 
+       5             : #include <mrs_uav_state_estimators/estimators/lateral/lat_generic.h>
+       6             : 
+       7             : //}
+       8             : 
+       9             : namespace mrs_uav_state_estimators
+      10             : {
+      11             : 
+      12             : /* initialize() //{*/
+      13          99 : void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      14             : 
+      15          99 :   ch_ = ch;
+      16          99 :   ph_ = ph;
+      17             : 
+      18          99 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      19             : 
+      20             :   // clang-format off
+      21          99 :   dt_ = 0.01;
+      22          99 :   input_coeff_ = 10;
+      23          99 :   default_input_coeff_ = 10;
+      24             : 
+      25          99 :   generateA();
+      26          99 :   generateB();
+      27             : 
+      28          99 :   H_ <<
+      29          99 :     1, 0, 0, 0, 0, 0,
+      30          99 :     0, 1, 0, 0, 0, 0;
+      31             : 
+      32          99 :   innovation_ << 
+      33          99 :     0, 0;
+      34             : 
+      35             : 
+      36             :   // clang-format on
+      37             : 
+      38             :   // | --------------- initialize parameter loader -------------- |
+      39             : 
+      40          99 :   if (is_core_plugin_) {
+      41             : 
+      42          99 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      43          99 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      44             :   }
+      45             : 
+      46          99 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      47             : 
+      48             :   // | --------------------- load parameters -------------------- |
+      49          99 :   ph->param_loader->loadParam("hdg_source_topic", hdg_source_topic_);
+      50          99 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      51          99 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      52          99 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      53          99 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      54          99 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      55          99 :   if (is_repredictor_enabled_) {
+      56           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      57             :   }
+      58             : 
+      59             :   // | --------------- corrections initialization --------------- |
+      60          99 :   ph->param_loader->loadParam("corrections", correction_names_);
+      61             : 
+      62         202 :   for (auto corr_name : correction_names_) {
+      63         103 :     corrections_.push_back(std::make_shared<Correction<lat_generic::n_measurements>>(
+      64        7852 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::LATERAL, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      65      198584 :         [this](const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      66      198584 :           return this->doCorrection(meas, R, state);
+      67             :         }));
+      68             :   }
+      69             : 
+      70             :   // | ------- check if all parameters loaded successfully ------ |
+      71          99 :   if (!ph->param_loader->loadedSuccessfully()) {
+      72           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      73           0 :     ros::shutdown();
+      74             :   }
+      75             : 
+      76          99 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      77             : 
+      78             :   // | ----------- initialize process noise covariance ---------- |
+      79          99 :   Q_ = Q_t::Zero();
+      80             :   double tmp_noise;
+      81          99 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      82          99 :   Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X)) = tmp_noise;
+      83          99 :   Q_(stateIdToIndex(POSITION, AXIS_Y), stateIdToIndex(POSITION, AXIS_Y)) = tmp_noise;
+      84          99 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      85          99 :   Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X)) = tmp_noise;
+      86          99 :   Q_(stateIdToIndex(VELOCITY, AXIS_Y), stateIdToIndex(VELOCITY, AXIS_Y)) = tmp_noise;
+      87          99 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      88          99 :   Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X)) = tmp_noise;
+      89          99 :   Q_(stateIdToIndex(ACCELERATION, AXIS_Y), stateIdToIndex(ACCELERATION, AXIS_Y)) = tmp_noise;
+      90             : 
+      91             :   // | ------------- initialize dynamic reconfigure ------------- |
+      92             :   drmgr_ =
+      93          99 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&LatGeneric::callbackReconfigure, this, _1, _2));
+      94          99 :   drmgr_->config.pos = Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X));
+      95          99 :   drmgr_->config.vel = Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X));
+      96          99 :   drmgr_->config.acc = Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X));
+      97          99 :   drmgr_->update_config(drmgr_->config);
+      98             : 
+      99             :   // | --------------- Kalman filter intialization -------------- |
+     100          99 :   const x_t        x0 = x_t::Zero();
+     101          99 :   const P_t        P0 = 1e3 * P_t::Identity();
+     102          99 :   const statecov_t sc0({x0, P0});
+     103          99 :   sc_ = sc0;
+     104             : 
+     105          99 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     106          99 :   if (is_repredictor_enabled_) {
+     107             : 
+     108           0 :     generateRepredictorModels(input_coeff_);
+     109             : 
+     110           0 :     const u_t       u0 = u_t::Zero();
+     111           0 :     const ros::Time t0 = ros::Time::now();
+     112           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
+     113             : 
+     114           0 :     setDt(1.0 / ch_->desired_uav_state_rate);
+     115             :   }
+     116             : 
+     117             :   // | ------------------ timers initialization ----------------- |
+     118          99 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerUpdate, this);  // not running after init
+     119             :   /* timer_check_health_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerCheckHealth, this); */
+     120             : 
+     121             :   // | --------------- subscribers initialization --------------- |
+     122             :   // subscriber to odometry
+     123         198 :   mrs_lib::SubscribeHandlerOptions shopts;
+     124          99 :   shopts.nh                 = nh;
+     125          99 :   shopts.node_name          = getPrintName();
+     126          99 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     127          99 :   shopts.threadsafe         = true;
+     128          99 :   shopts.autostart          = true;
+     129          99 :   shopts.queue_size         = 10;
+     130          99 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     131             : 
+     132          99 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     133             :   sh_hdg_state_ =
+     134          99 :       mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput>(shopts, hdg_source_topic_);  // for transformation of desired accelerations from body to global frame
+     135             : 
+     136             :   // | ---------------- publishers initialization --------------- |
+     137          99 :   if (ch_->debug_topics.input) {
+     138          99 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     139             :   }
+     140          99 :   if (ch_->debug_topics.output) {
+     141          99 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     142             :   }
+     143          99 :   if (ch_->debug_topics.diag) {
+     144           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+     145             :   }
+     146             : 
+     147             :   // | ------------------ finish initialization ----------------- |
+     148             : 
+     149          99 :   if (changeState(INITIALIZED_STATE)) {
+     150          99 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+     151             :   } else {
+     152           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+     153             :   }
+     154          99 : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ start() */
+     158        3930 : bool LatGeneric::start(void) {
+     159             : 
+     160        3930 :   if (isInState(READY_STATE)) {
+     161             :     /* timer_update_.start(); */
+     162          99 :     changeState(STARTED_STATE);
+     163          99 :     return true;
+     164             : 
+     165             :   } else {
+     166        3831 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     167        3831 :     return false;
+     168             :   }
+     169             : }
+     170             : /*//}*/
+     171             : 
+     172             : /*//{ pause() */
+     173           0 : bool LatGeneric::pause(void) {
+     174             : 
+     175           0 :   if (isInState(RUNNING_STATE)) {
+     176           0 :     changeState(STOPPED_STATE);
+     177           0 :     return true;
+     178             : 
+     179             :   } else {
+     180           0 :     return false;
+     181             :   }
+     182             : }
+     183             : /*//}*/
+     184             : 
+     185             : /*//{ reset() */
+     186           0 : bool LatGeneric::reset(void) {
+     187             : 
+     188           0 :   if (!isInitialized()) {
+     189           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     190           0 :     return false;
+     191             :   }
+     192             : 
+     193           0 :   changeState(STOPPED_STATE);
+     194             : 
+     195             :   // Initialize LKF state and covariance
+     196           0 :   const x_t        x0 = x_t::Zero();
+     197           0 :   const P_t        P0 = 1e6 * P_t::Identity();
+     198           0 :   const statecov_t sc0({x0, P0});
+     199           0 :   sc_ = sc0;
+     200             : 
+     201             :   // Instantiate the LKF itself
+     202           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     203           0 :   if (is_repredictor_enabled_) {
+     204             : 
+     205           0 :     const u_t       u0 = u_t::Zero();
+     206           0 :     const ros::Time t0 = ros::Time(0);
+     207           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_[0], rep_buffer_size_);
+     208             :   }
+     209             : 
+     210           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     211             : 
+     212           0 :   return true;
+     213             : }
+     214             : /*//}*/
+     215             : 
+     216             : /* timerUpdate() //{*/
+     217      193503 : void LatGeneric::timerUpdate(const ros::TimerEvent &event) {
+     218             : 
+     219      193503 :   if (!isInitialized()) {
+     220       21420 :     return;
+     221             :   }
+     222             : 
+     223      193501 :   switch (getCurrentSmState()) {
+     224             : 
+     225           0 :     case UNINITIALIZED_STATE: {
+     226           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     227           0 :       break;
+     228             :     }
+     229             : 
+     230          68 :     case READY_STATE: {
+     231          68 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     232          68 :       break;
+     233             :     }
+     234             : 
+     235        6530 :     case INITIALIZED_STATE: {
+     236             :       // initialize the estimator with current corrections
+     237        6704 :       for (auto correction : corrections_) {
+     238        6605 :         auto res = correction->getProcessedCorrection();
+     239        6605 :         if (res) {
+     240         174 :           auto measurement_stamped = res.value(); 
+     241         174 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     242         174 :           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
+     243         174 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+     244             :                             measurement_stamped.value(AXIS_Y));
+     245             :         } else {
+     246        6431 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getNamespacedName().c_str());
+     247        6431 :           return;
+     248             :         }
+     249             :       }
+     250          99 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     251          99 :       changeState(READY_STATE);
+     252          99 :       break;
+     253             :     }
+     254             : 
+     255          99 :     case STARTED_STATE: {
+     256          99 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     257          99 :       if (isConverged()) {
+     258          99 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     259          99 :         changeState(RUNNING_STATE);
+     260             :       }
+     261          99 :       break;
+     262             :     }
+     263             : 
+     264      186806 :     case RUNNING_STATE: {
+     265      384867 :       for (auto correction : corrections_) {
+     266      198052 :         if (!correction->isHealthy()) {
+     267           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     268           0 :           changeState(ERROR_STATE);
+     269             :         }
+     270             :       }
+     271      186809 :       break;
+     272             :     }
+     273             : 
+     274           0 :     case STOPPED_STATE: {
+     275           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     276           0 :       break;
+     277             :     }
+     278             : 
+     279           0 :     case ERROR_STATE: {
+     280           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     281             : 
+     282           0 :       ros::Time t_now = ros::Time::now();
+     283           0 :       if (is_error_state_first_time_) {
+     284           0 :         prev_time_in_error_state_  = t_now;
+     285           0 :         is_error_state_first_time_ = false;
+     286           0 :         error_state_duration_      = ros::Duration(0.0);
+     287             :       }
+     288           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     289             : 
+     290             : 
+     291             :       // check if all corrections are healthy now
+     292           0 :       bool all_corrections_healthy = true;
+     293           0 :       for (auto correction : corrections_) {
+     294           0 :         if (!correction->isHealthy()) {
+     295           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     296           0 :           all_corrections_healthy = false;
+     297             :         }
+     298             :       }
+     299             : 
+     300           0 :       if (all_corrections_healthy && innovation_ok_) {
+     301             :         // initialize the estimator again if corrections become healthy
+     302           0 :         if (error_state_duration_.toSec() > 5.0) {
+     303           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     304           0 :           changeState(INITIALIZED_STATE);
+     305           0 :           is_error_state_first_time_ = true;
+     306           0 :         }
+     307             :       } else {
+     308           0 :         is_error_state_first_time_ = true;
+     309             :       }
+     310             : 
+     311           0 :       prev_time_in_error_state_ = t_now;
+     312             : 
+     313           0 :       break;
+     314             :     }
+     315             :   }
+     316             : 
+     317      187079 :   if (sh_control_input_.newMsg()) {
+     318       92524 :     is_input_ready_ = true;
+     319             :   }
+     320             : 
+     321             :   // check age of input
+     322      187074 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {  // TODO: parametrize, if older than say 1 second, eland
+     323          16 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f s), using zero input instead", getPrintName().c_str(),
+     324             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     325          16 :     is_input_ready_ = false;
+     326             :   }
+     327             : 
+     328      187066 :   if (fun_get_hdg_()) {
+     329      186799 :     is_hdg_state_ready_ = true;
+     330             :   }
+     331             : 
+     332      187076 :   if (!isRunning() && !isStarted()) {
+     333         148 :     return;
+     334             :   }
+     335             : 
+     336      186903 :   if (first_iter_) {
+     337          99 :     first_iter_ = false;
+     338          99 :     return;
+     339             :   }
+     340             : 
+     341             :   // obtain dt for state prediction
+     342      186804 :   double dt = (event.current_real - event.last_real).toSec();
+     343      186796 :   if (dt <= 0.0 || dt > 1.0) {  // sometimes the timer ticks twice simultaneously in simulation - we ignore the second tick, in case of stopping and starting the timer, the last_real is 0
+     344       14742 :     return;
+     345             :   }
+     346             : 
+     347      172054 :   if (!is_repredictor_enabled_) { // repredictor calculates dt on its own
+     348      172052 :     setDt(dt);
+     349             :   }
+     350             : 
+     351             :   // obtain unbiased desired control acceleration in the estimator frame that will be used as input to the estimator
+     352      172048 :   u_t       u;
+     353      172056 :   ros::Time input_stamp;
+     354      172049 :   if (is_input_ready_ && is_hdg_state_ready_) {
+     355      104641 :     auto res = fun_get_hdg_();
+     356      104653 :     if (!res) {
+     357           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not obtain heading", getPrintName().c_str());
+     358           0 :       return;
+     359             :     }
+     360             : 
+     361      104654 :     const tf2::Vector3 des_acc_global = getAccGlobal(sh_control_input_.getMsg(), res.value());
+     362      104577 :     input_stamp                       = sh_control_input_.getMsg()->header.stamp;
+     363      104620 :     if (input_coeff_ != default_input_coeff_){
+     364          77 :       setInputCoeff(default_input_coeff_);
+     365             :     }
+     366      104620 :     u(0) = des_acc_global.getX();
+     367      104591 :     u(1) = des_acc_global.getY();
+     368             : 
+     369             :   } else {  // this is ok before the controller starts controlling but bad during actual flight (causes delayed estimated acceleration and velocity)
+     370       67377 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     371       67390 :     input_stamp = ros::Time::now();
+     372       67391 :     if (input_coeff_ != 0){
+     373         115 :       setInputCoeff(0);
+     374             :     }
+     375       67391 :     u = u_t::Zero();
+     376             :   }
+     377             : 
+     378             :   // go through available corrections and apply them
+     379             :   /* for (auto correction : corrections_) { */
+     380             :   /*   auto res = correction->getProcessedCorrection(); */
+     381             :   /*   if (res) { */
+     382             :   /*     auto measurement_stamped = res.value(); */
+     383             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     384             :   /*   } */
+     385             :   /* } */
+     386             : 
+     387             :   // get current state, covariance, and process noise
+     388      171998 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     389      172023 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     390             : 
+     391             :   // prediction step
+     392             :   try {
+     393             :     // Apply the prediction step
+     394      344085 :     std::scoped_lock lock(mutex_lkf_);
+     395      172027 :     if (is_repredictor_enabled_) {
+     396           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
+     397           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     398             :     } else {
+     399      172027 :       sc = lkf_->predict(sc, u, Q, dt_);
+     400             :     }
+     401             :   }
+     402           0 :   catch (const std::exception &e) {
+     403           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     404           0 :     changeState(ERROR_STATE);
+     405             :   }
+     406             : 
+     407             :   // update the state and covariance variable that is queried by the estimation manager
+     408      172019 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     409             : 
+     410             :   // publishing
+     411      172014 :   publishInput(u, input_stamp);
+     412      172072 :   publishOutput();
+     413      172072 :   publishDiagnostics();
+     414             : }
+     415             : /*//}*/
+     416             : 
+     417             : /*//{ timerCheckHealth() */
+     418           0 : void LatGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     419             : 
+     420           0 :   if (!isInitialized()) {
+     421           0 :     return;
+     422             :   }
+     423             : 
+     424           0 :   switch (getCurrentSmState()) {
+     425             : 
+     426           0 :     case UNINITIALIZED_STATE: {
+     427           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     428           0 :       break;
+     429             :     }
+     430             : 
+     431           0 :     case READY_STATE: {
+     432           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     433           0 :       break;
+     434             :     }
+     435             : 
+     436           0 :     case INITIALIZED_STATE: {
+     437             :       // initialize the estimator with current corrections
+     438           0 :       for (auto correction : corrections_) {
+     439           0 :         auto res = correction->getProcessedCorrection();
+     440           0 :         if (res) {
+     441           0 :           auto measurement_stamped = res.value();
+     442           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     443           0 :           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
+     444           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+     445             :                             measurement_stamped.value(AXIS_Y));
+     446             :         } else {
+     447           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getPrintName().c_str());
+     448           0 :           return;
+     449             :         }
+     450             :       }
+     451           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     452           0 :       changeState(READY_STATE);
+     453           0 :       break;
+     454             :     }
+     455             : 
+     456           0 :     case STARTED_STATE: {
+     457           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     458           0 :       if (isConverged()) {
+     459           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     460           0 :         changeState(RUNNING_STATE);
+     461             :       }
+     462           0 :       break;
+     463             :     }
+     464             : 
+     465           0 :     case RUNNING_STATE: {
+     466           0 :       for (auto correction : corrections_) {
+     467           0 :         if (!correction->isHealthy()) {
+     468           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     469           0 :           changeState(ERROR_STATE);
+     470             :         }
+     471             :       }
+     472           0 :       break;
+     473             :     }
+     474             : 
+     475           0 :     case STOPPED_STATE: {
+     476           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     477           0 :       break;
+     478             :     }
+     479             : 
+     480           0 :     case ERROR_STATE: {
+     481           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     482           0 :       bool all_corrections_healthy = true;
+     483           0 :       for (auto correction : corrections_) {
+     484           0 :         if (!correction->isHealthy()) {
+     485           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     486           0 :           correction->resetProcessors();
+     487           0 :           all_corrections_healthy = false;
+     488             :         }
+     489             :       }
+     490             :       // initialize the estimator again if corrections become healthy
+     491           0 :       if (all_corrections_healthy) {
+     492           0 :         changeState(INITIALIZED_STATE);
+     493             :       }
+     494           0 :       break;
+     495             :     }
+     496             :   }
+     497             : 
+     498           0 :   if (sh_control_input_.newMsg()) {
+     499           0 :     is_input_ready_ = true;
+     500             :   }
+     501             : 
+     502             :   // check age of input
+     503           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {  // TODO: parametrize, if older than say 1 second, eland
+     504           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f s), using zero input instead", getPrintName().c_str(),
+     505             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     506           0 :     is_input_ready_ = false;
+     507             :   }
+     508             : 
+     509           0 :   if (fun_get_hdg_()) {
+     510           0 :     is_hdg_state_ready_ = true;
+     511             :   }
+     512             : }
+     513             : /*//}*/
+     514             : 
+     515             : /*//{ doCorrection() */
+     516      198606 : void LatGeneric::doCorrection(const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     517      198606 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     518      198597 : }
+     519             : /*//}*/
+     520             : 
+     521             : /*//{ doCorrection() */
+     522      198520 : void LatGeneric::doCorrection(const z_t &z, const double R, const StateId_t &state_id, const ros::Time &meas_stamp) {
+     523             : 
+     524      198520 :   if (!isInitialized()) {
+     525         156 :     return;
+     526             :   }
+     527             : 
+     528             :   // we do not want to perform corrections until the estimator is initialized
+     529      198528 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     530         153 :     return;
+     531             :   }
+     532             : 
+     533             :   // for position state check the innovation
+     534      198295 :   if (state_id == POSITION) {
+     535             :     {
+     536      187552 :       std::scoped_lock lock(mtx_innovation_);
+     537             : 
+     538      187599 :       is_mitigating_jump_ = false;
+     539      187620 :       innovation_(0)      = z(0) - getState(POSITION, AXIS_X);
+     540      187555 :       innovation_(1)      = z(1) - getState(POSITION, AXIS_Y);
+     541             : 
+     542      187595 :       if (fabs(innovation_(0)) > pos_innovation_limit_ || fabs(innovation_(1)) > pos_innovation_limit_) {
+     543           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - [%.2f %.2f] lim: %.2f", getPrintName().c_str(), innovation_(0), innovation_(1),
+     544             :                           pos_innovation_limit_);
+     545           0 :         innovation_ok_ = false;
+     546           0 :         switch (exc_innovation_action_) {
+     547           0 :           case ExcInnoAction_t::ELAND: {
+     548           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger eland in control manager", getPrintName().c_str());
+     549           0 :             changeState(ERROR_STATE);
+     550           0 :             break;
+     551             :           }
+     552           0 :           case ExcInnoAction_t::SWITCH: {
+     553           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: innovation should trigger estimator switch but no eland", getPrintName().c_str());
+     554           0 :             innovation_(0) = 0.0;  // this is quite hacky but is there other way to switch estimators and not trigger eland by the large innovation?
+     555           0 :             innovation_(1) = 0.0;
+     556           0 :             changeState(ERROR_STATE);
+     557           0 :             break;
+     558             :           }
+     559           0 :           case ExcInnoAction_t::MITIGATE: {
+     560           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger estimate jump mitigation", getPrintName().c_str());
+     561           0 :             innovation_(0)      = 0.0;  // this is quite hacky but is there other way to switch estimators and not trigger eland by the large innovation?
+     562           0 :             innovation_(1)      = 0.0;
+     563           0 :             is_mitigating_jump_ = true;
+     564           0 :             setState(z(0), POSITION, AXIS_X);
+     565           0 :             setState(z(1), POSITION, AXIS_Y);
+     566           0 :             break;
+     567             :           }
+     568           0 :           case ExcInnoAction_t::NONE: {
+     569           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation ignored", getPrintName().c_str());
+     570           0 :             break;
+     571             :           }
+     572             :         }
+     573             :       }
+     574      187602 :       innovation_ok_ = true;
+     575             :     }
+     576             :   }
+     577             : 
+     578      198315 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     579             :   try {
+     580             :     // Apply the correction step
+     581      396701 :     std::scoped_lock lock(mutex_lkf_);
+     582      198405 :     H_                           = H_t::Zero();
+     583      198260 :     H_(AXIS_X, state_id * 2)     = 1;
+     584      198335 :     H_(AXIS_Y, state_id * 2 + 1) = 1;
+     585      198370 :     lkf_->H                      = H_;
+     586             : 
+     587      198198 :     if (is_repredictor_enabled_) {
+     588             : 
+     589           0 :       lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[state_id]);
+     590             :     } else {
+     591      198198 :       sc = lkf_->correct(sc, z, R_t::Ones() * R);
+     592             :     }
+     593             :   }
+     594           0 :   catch (const std::exception &e) {
+     595             :     // In case of error, alert the user
+     596           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     597             :   }
+     598             : 
+     599      198141 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     600             : }  // namespace mrs_uav_state_estimators
+     601             : /*//}*/
+     602             : 
+     603             : /*//{ isConverged() */
+     604          99 : bool LatGeneric::isConverged() {
+     605             : 
+     606             :   // TODO: check convergence by rate of change of determinant
+     607             : 
+     608          99 :   return true;
+     609             : }
+     610             : /*//}*/
+     611             : 
+     612             : /*//{ getState() */
+     613     1484208 : double LatGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     614     1484208 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     615             : }
+     616             : 
+     617     1484067 : double LatGeneric::getState(const int &state_idx_in) const {
+     618     1484067 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     619             : }
+     620             : /*//}*/
+     621             : 
+     622             : /*//{ setState() */
+     623         348 : void LatGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     624         348 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     625         348 : }
+     626             : 
+     627         348 : void LatGeneric::setState(const double &state_in, const int &state_idx_in) {
+     628         348 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     629         348 : }
+     630             : /*//}*/
+     631             : 
+     632             : /*//{ getStates() */
+     633      172070 : LatGeneric::states_t LatGeneric::getStates(void) const {
+     634      344132 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     635             : }
+     636             : /*//}*/
+     637             : 
+     638             : /*//{ setStates() */
+     639           0 : void LatGeneric::setStates(const states_t &states_in) {
+     640           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     641           0 : }
+     642             : /*//}*/
+     643             : 
+     644             : /*//{ getCovariance() */
+     645      734223 : double LatGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     646      734223 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     647             : }
+     648             : 
+     649      734220 : double LatGeneric::getCovariance(const int &state_idx_in) const {
+     650      734220 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     651             : }
+     652             : /*//}*/
+     653             : 
+     654             : /*//{ getCovarianceMatrix() */
+     655      172069 : LatGeneric::covariance_t LatGeneric::getCovarianceMatrix(void) const {
+     656      344129 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     657             : }
+     658             : /*//}*/
+     659             : 
+     660             : /*//{ setCovarianceMatrix() */
+     661           0 : void LatGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     662           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     663           0 : }
+     664             : /*//}*/
+     665             : 
+     666             : /*//{ getInnovation() */
+     667      367108 : double LatGeneric::getInnovation(const int &state_idx) const {
+     668      367108 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     669             : }
+     670             : 
+     671      367112 : double LatGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     672      367112 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     673             : }
+     674             : /*//}*/
+     675             : 
+     676             : /*//{ setDt() */
+     677      172070 : void LatGeneric::setDt(const double &dt) {
+     678      172070 :   dt_ = dt;
+     679      172070 :   generateA();
+     680      172069 :   generateB();
+     681      344104 :   std::scoped_lock lock(mutex_lkf_);
+     682      172050 :   lkf_->A = A_;
+     683      172060 :   lkf_->B = B_;
+     684      172030 : }
+     685             : /*//}*/
+     686             : 
+     687             : /*//{ setInputCoeff() */
+     688         192 : void LatGeneric::setInputCoeff(const double &input_coeff) {
+     689         192 :   input_coeff_ = input_coeff;
+     690         192 :   generateA();
+     691         192 :   generateB();
+     692         384 :   std::scoped_lock lock(mutex_lkf_);
+     693         192 :   lkf_->A = A_;
+     694         192 :   lkf_->B = B_;
+     695             : 
+     696         192 :   if (is_repredictor_enabled_) {
+     697           0 :     models_.clear();
+     698           0 :     generateRepredictorModels(input_coeff_);
+     699             :   }
+     700         192 : }
+     701             : /*//}*/
+     702             : 
+     703             : /*//{ generateRepredictorModels() */
+     704           0 : void LatGeneric::generateRepredictorModels(const double input_coeff) {
+     705           0 :     for (int i = 0; (int)(i < lat_generic::n_states / 2); i++) {
+     706             : 
+     707           0 :       auto lambda_generateA = [input_coeff](const double dt) {
+     708           0 :         A_t A;
+     709             :         // clang-format off
+     710           0 :         A <<
+     711           0 :           1, 0, dt, 0, 0.5 * dt * dt, 0,
+     712           0 :           0, 1, 0, dt, 0, 0.5 * dt * dt,
+     713           0 :           0, 0, 1, 0, dt, 0,
+     714           0 :           0, 0, 0, 1, 0, dt,
+     715           0 :           0, 0, 0, 0, 1-(input_coeff * dt), 0,
+     716           0 :           0, 0, 0, 0, 0, 1-(input_coeff * dt);
+     717             :         // clang-format on
+     718           0 :         return A;
+     719           0 :       };
+     720             : 
+     721           0 :       auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
+     722           0 :         B_t B = B.Zero();
+     723             :         // clang-format off
+     724           0 :           B <<
+     725           0 :             0, 0,
+     726           0 :             0, 0,
+     727           0 :             0, 0,
+     728           0 :             0, 0,
+     729           0 :             input_coeff * dt, 0,
+     730           0 :             0, input_coeff * dt;
+     731             :         // clang-format on
+     732             : 
+     733           0 :         return B;
+     734           0 :       };
+     735             : 
+     736           0 :       H_t H                = H.Zero();
+     737           0 :       H(AXIS_X, i * 2)     = 1;
+     738           0 :       H(AXIS_Y, i * 2 + 1) = 1;
+     739           0 :       models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
+     740             :     }
+     741           0 : }
+     742             : /*//}*/
+     743             : 
+     744             : /*//{ generateA() */
+     745      172353 : void LatGeneric::generateA() {
+     746             : 
+     747             :   // clang-format off
+     748      172353 :     A_ <<
+     749      172307 :       1, 0, dt_, 0, 0.5 * dt_ * dt_, 0,
+     750      172345 :       0, 1, 0, dt_, 0, 0.5 * dt_ * dt_,
+     751      172344 :       0, 0, 1, 0, dt_, 0,
+     752      172338 :       0, 0, 0, 1, 0, dt_,
+     753      172350 :       0, 0, 0, 0, 1-(input_coeff_ * dt_), 0,
+     754      172352 :       0, 0, 0, 0, 0, 1-(input_coeff_ * dt_);
+     755             :   // clang-format on
+     756      172353 : }
+     757             : /*//}*/
+     758             : 
+     759             : /*//{ generateB() */
+     760      172360 : void LatGeneric::generateB() {
+     761             : 
+     762             :   // clang-format off
+     763      172360 :     B_ <<
+     764      172352 :       0, 0,
+     765      172318 :       0, 0,
+     766      172327 :       0, 0,
+     767      172337 :       0, 0,
+     768      172341 :       input_coeff_ * dt_, 0,
+     769      172343 :       0, input_coeff_ * dt_;
+     770             :   // clang-format on
+     771      172332 : }
+     772             : /*//}*/
+     773             : 
+     774             : /*//{ callbackReconfigure() */
+     775          99 : void LatGeneric::callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     776             : 
+     777          99 :   if (!isInitialized()) {
+     778          99 :     return;
+     779             :   }
+     780             : 
+     781           0 :   Q_t Q;
+     782           0 :   Q(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X))         = config.pos;
+     783           0 :   Q(stateIdToIndex(POSITION, AXIS_Y), stateIdToIndex(POSITION, AXIS_Y))         = config.pos;
+     784           0 :   Q(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X))         = config.vel;
+     785           0 :   Q(stateIdToIndex(VELOCITY, AXIS_Y), stateIdToIndex(VELOCITY, AXIS_Y))         = config.vel;
+     786           0 :   Q(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X)) = config.acc;
+     787           0 :   Q(stateIdToIndex(ACCELERATION, AXIS_Y), stateIdToIndex(ACCELERATION, AXIS_Y)) = config.acc;
+     788           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     789             : }
+     790             : /*//}*/
+     791             : 
+     792             : /*//{ getNamespacedName() */
+     793         598 : std::string LatGeneric::getNamespacedName() const {
+     794        1196 :   return parent_state_est_name_ + "/" + getName();
+     795             : }
+     796             : /*//}*/
+     797             : 
+     798             : /*//{ getPrintName() */
+     799         931 : std::string LatGeneric::getPrintName() const {
+     800        1862 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     801             : }
+     802             : /*//}*/
+     803             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..19d9bca23e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html @@ -0,0 +1,221 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..f1e8812fb983e0976237d0a73def532dd444fd01 GIT binary patch literal 3149 zcmV-T46^fyP)_0;S;5*sS;5}G*?*N6>jy}HP|j}BK3%VuODPfv5I;H7{eGX*-}^kB zG48j3Z@!;`Ens|&1F1ZQF`h$ewZ_@9q|;}f5^v`220k9{(F`BnB|n%JOfL-8W_M8c5pV}FAI$(T3#;?-=&1;OZZ(E}Y zqjl;x6no%zdKj}Ey%mEsgLR#Ujne{_!=1yP1?hq?f}#%qnxt`LXM~v#6X)?8L$(K< zaJO`nz*}W!B%)znpZKVP0ZR-RBe)1ypDF~U9-;XyCU`HO$7u*Z2Svh6q zJ2q*UkCBlb6Fn8lu>i6o&LaVO0Ksp#%9;%T{)VX;oawCrz+s^vQUL}MoR0&5kJ#8k(Tdk}MW0$>Ho?hG%u<9y7<3A2@+Gco3JS&s#b1Q2vy07NuO%9tV~12JZ* z7iLQwvjDrRV4j-yDLB(?90B9&Ab7oeZd>w56u@vsY@2dGq&0@u0LYB7HNNKE>F>Ae zyw;N<(TU^?(_@NJM50VZxKfHgNo zK%X!QbGK7>0lk%&fVAClUjU5*O|8if*l^#ABb5Uw4VN)m*NDVyR>f?<>={T7ho&`b zuBH~*l%o$NB#h_FyHm>>1U9@jXpMdRoHa9#Hu~Txhj7oAZ&3yvjqayIpVa(K%g6NX zh-Wq&(TwuZxYf(_u%ujGts~7`4{WTiLD-bt3 z?3j%pmd+<4UCd*YHTEs8)l!(1taD1gG0b8{RcMoS#->^aAi?-Delo_6n@0e*UY25H zjb`b|Q}-;3-S+(Ng+dgFgt1JUY(1F%Cpt$j*ctOM%I%0eW`})F-|j*&w*+xpb}FQ) zF(5b~0$@H$cbS|Q;DN?K%qZ2b@3)Xv&=_7tm?5pnt&xC_|O{ zWBL|*o+it@PDWbRE6g6`s&v4np5V~G$OC)JPES~4s=pP7 zxzJGq=0YGe#_$-!PX#wOn=uFRs^ZSw`OuRF2v~(~wD19rB z4WN?xH1g-yFx+l?3<<;N_u!K!`NhYC=pZA6PXSs-i&-SWW?ULJwb*fjOW@O z^Qegi$DZ;Cv1dp5{nDtkXSrbb7%8uLPS^u!qkw&@ZBJ8Xm$Z;kntx&V0jDL_D0})d z4`|`g78;~%D3@}&u|K^XudZ`R9tf&)>3#VZMC{R)9oDRqDgk0Vki`UGT#>{_Uo(A6 z_cd@l0~~7bVT|F&Kgu3NbuVH^+pI<(*R9ccb*Z0C49PBayTG>L^LZ zkPbsKO~sq^u)AaTe1vvO&i1$me1R%_cnb?HE~|L32|hL9U2%M-iD$JguL=sq3WH3b1((KcHF90Ve0m`dG^@g>?6ta?Op}g^)Z{e@ftf)yI;xJyV{O4 z?bpLsZ5bhGh%vXvB}cr67j&!G6+sn@EtPd}mSRJ4&8LLTb?uxDbSGm(y2k7^L=#@> z>l(;ETG_p#tVjsy<*aMrNlq?2V*&ti>q)LaNzs4Cctww9)$tJ@VvucDmMi933?HnF zt3r2)q?c2TkW{(m0DI?*xkvM=J}Jo@qw%a0Qb@KocsY_89_o`a$mB&`B#7>lnr!|| z!@$4P6-s@C_BDS4F*-ev@P#xk&`|^06X%EhW^}9t-1>)C&f@|;p`(k<47XAi#q2%6 ztsF_Yp*_A;=}e2ZtnnD8h5>f!d?<4H@P0?-dUjobW6s_v!d~l)+B5tOIx}1)O@)u0 z2oR~JXi{F63$~(;Ri|HDS5WHLBh@_<;71w$B}Ka*Y#3O7$<9=A`}|)t4E(xk^HX5| zhs)Bafj&L?pzKZ@>{RI|D4=4^KUL z>*-+w;j0By*Z4K;Vfka->x$|%6pGd$(wQ%bRV<)RbzYw0UISnW2DzJ`!+`9{YQXmx zuEksm!!wQx)_4r7&oz3-$Ego?xS$MZ9wXIet8aEe$(Ot#J)E#S$tIqe2cXt6m;sNL zFBK5|OnMCT(1Ubs^=mW6wTi>{Y%n&^nxr%D)~}Nn!dx5tWc@lHLAftXGAUq7^g{RL zkMI=B(w>A*vMyX17xvn59yYlht+C%q*pch{5OoX><1=)=qIBNvFM3oQ_oc@G-#PZa zzMc*iypke2)#f3tvAUkjt`YtJx9vCi+^QPk;X zchHrsERDf}eQee62kawWqesc7j(yDP_#N`t-5j2XRvCYl7Ivv-TirI$v+Q}ZT3Bvz z{vj>BYReyKt>p2{%0eA$K)oAYdN3Q4vqE5oY~eG(^7kA$_=#YfT1w3hvy-7 z03}=uAdiayOk1{bs4I+x?L1)NqdYUz;w;^>26mqzJw<$DALyQ>Cqnev@$AxL5uaSr zFtO*ufJKwL7_)?_fF|iNkF>&1iCGD`Vwgs9Hb)6@xK3mk;)-_5>sN zoE%^R=zW;yH~2k&NX)(;Fbj<)7^t;K|HP_aC1!@a8Jm1aDhV;x-)cfMdFyMFH-jWM ze&f3U8PN3B+kl(cG4#pjhvTO-XvP#InM!sooOLD+J^`;5vyW%uOwGj&)pbFNq1w?W nL1PwNK=pkpj}n5dWZ?b-?P9VG7y)KU00000NkvXXu0mjfFty|x literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html new file mode 100644 index 0000000000..9f8ee9b1db --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()20
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()20
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()20
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().220
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html new file mode 100644 index 0000000000..fd14feb084 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()20
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()20
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()20
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().220
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.frameset.html new file mode 100644 index 0000000000..cdfd029a0c --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html new file mode 100644 index 0000000000..0d4dde6345 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_state_estimators/estimators/state/state_generic.h>
+       2             : 
+       3             : namespace mrs_uav_state_estimators
+       4             : {
+       5             : 
+       6             : namespace gps_baro
+       7             : {
+       8             : 
+       9             : const char estimator_name[] = "gps_baro";
+      10             : const bool is_core_plugin = true;
+      11             : 
+      12             : class GpsBaro : public StateGeneric {
+      13             : public:
+      14          20 :   GpsBaro() : StateGeneric(estimator_name, is_core_plugin) {
+      15          20 :   }
+      16             : 
+      17          40 :   ~GpsBaro(void) {
+      18          40 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_baro
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          20 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::gps_baro::GpsBaro, mrs_uav_managers::StateEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html new file mode 100644 index 0000000000..2b03dc176f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html @@ -0,0 +1,27 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..32cb0dad19f40488be4eb0ccd03a6cbd7fc1ac32 GIT binary patch literal 231 zcmeAS@N?(olHy`uVBq!ia0vp^0YEIt!VDxsGPYO&DTx4|5ZC|z|E~gq + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()75
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()75
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()75
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().275
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html new file mode 100644 index 0000000000..4903609c81 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()75
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()75
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()75
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().275
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.frameset.html new file mode 100644 index 0000000000..716d11d216 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html new file mode 100644 index 0000000000..f36e7f3747 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_state_estimators/estimators/state/state_generic.h>
+       2             : 
+       3             : namespace mrs_uav_state_estimators
+       4             : {
+       5             : 
+       6             : namespace gps_garmin
+       7             : {
+       8             : 
+       9             : const char estimator_name[] = "gps_garmin";
+      10             : const bool is_core_plugin = true;
+      11             : 
+      12             : class GpsGarmin : public StateGeneric {
+      13             : public:
+      14          75 :   GpsGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+      15          75 :   }
+      16             : 
+      17         150 :   ~GpsGarmin(void) {
+      18         150 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_garmin
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          75 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::gps_garmin::GpsGarmin, mrs_uav_managers::StateEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.overview.html new file mode 100644 index 0000000000..88be0074b4 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.overview.html @@ -0,0 +1,27 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..606fb073ca70818dc0b3b5dba84fd6a7044d0512 GIT binary patch literal 231 zcmeAS@N?(olHy`uVBq!ia0vp^0YEIt!VDxsGPYO&DTx4|5ZC|z|E~gq^JNDtZ&EXPgU2N!e?RY*!@G!k?Ta()7BTAKis#>5!bv}um4PI*Z02~7mGQc zuCNw3Xkx0Ko_*uhD$z@81X?onZt12fEV|g{c9?_v+hwVbPc627Ikr{%)|^j!tT$gZ WJ^4%e>pP$W7(8A5T-G@yGywov30pG& literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html new file mode 100644 index 0000000000..8c03d8327a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-25 05:42:57Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html new file mode 100644 index 0000000000..50f7aa0d12 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-25 05:42:57Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-detail.html b/mrs_uav_state_estimators/src/estimators/state/index-detail.html new file mode 100644 index 0000000000..3f1be527b4 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-25 05:42:57Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/state/index-sort-f.html new file mode 100644 index 0000000000..646034d599 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-sort-f.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-25 05:42:57Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html new file mode 100644 index 0000000000..be42ceeb39 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-25 05:42:57Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index.html b/mrs_uav_state_estimators/src/estimators/state/index.html new file mode 100644 index 0000000000..4ea2476519 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-25 05:42:57Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html new file mode 100644 index 0000000000..37c52784c6 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9712776.4 %
Date:2024-04-25 05:42:57Functions:5955.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::passthrough::Passthrough::isConverged()0
mrs_uav_state_estimators::passthrough::Passthrough::setUavState(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_state_estimators::passthrough::Passthrough::pause()0
mrs_uav_state_estimators::passthrough::Passthrough::reset()0
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_state_estimators::passthrough::Passthrough::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)1
mrs_uav_state_estimators::passthrough::Passthrough::start()1
mrs_uav_state_estimators::passthrough::Passthrough::timerUpdate(ros::TimerEvent const&)1055
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckHealth(ros::TimerEvent const&)1071
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html new file mode 100644 index 0000000000..148cc6fa6c --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9712776.4 %
Date:2024-04-25 05:42:57Functions:5955.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_state_estimators::passthrough::Passthrough::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)1
mrs_uav_state_estimators::passthrough::Passthrough::isConverged()0
mrs_uav_state_estimators::passthrough::Passthrough::setUavState(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_state_estimators::passthrough::Passthrough::timerUpdate(ros::TimerEvent const&)1055
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckHealth(ros::TimerEvent const&)1071
mrs_uav_state_estimators::passthrough::Passthrough::pause()0
mrs_uav_state_estimators::passthrough::Passthrough::reset()0
mrs_uav_state_estimators::passthrough::Passthrough::start()1
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.frameset.html new file mode 100644 index 0000000000..cf08741b5f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/passthrough.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html new file mode 100644 index 0000000000..fdaa4e2f8e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html @@ -0,0 +1,343 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/passthrough.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9712776.4 %
Date:2024-04-25 05:42:57Functions:5955.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <mrs_uav_state_estimators/estimators/state/passthrough.h>
+       4             : 
+       5             : //}
+       6             : 
+       7             : namespace mrs_uav_state_estimators
+       8             : {
+       9             : 
+      10             : namespace passthrough
+      11             : {
+      12             : 
+      13             : /* initialize() //{*/
+      14           1 : void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16           1 :   ch_ = ch;
+      17           1 :   ph_ = ph;
+      18             : 
+      19           1 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // | --------------- param loader initialization -------------- |
+      22             : 
+      23           1 :   if (is_core_plugin_) {
+      24             : 
+      25           1 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      26           1 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      27             :   }
+      28             : 
+      29           1 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      30             : 
+      31             :   // | --------------------- load parameters -------------------- |
+      32           1 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      33           1 :   ph->param_loader->loadParam("message/topic", msg_topic_);
+      34           1 :   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
+      35             : 
+      36             :   // | ------------------ timers initialization ----------------- |
+      37           1 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerUpdate, this, false, false);  // not running after init
+      38           1 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerCheckHealth, this);
+      39             : 
+      40             :   // | --------------- subscribers initialization --------------- |
+      41           2 :   mrs_lib::SubscribeHandlerOptions shopts;
+      42           1 :   shopts.nh                 = nh;
+      43           1 :   shopts.node_name          = getPrintName();
+      44           1 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      45           1 :   shopts.threadsafe         = true;
+      46           1 :   shopts.autostart          = true;
+      47           1 :   shopts.queue_size         = 10;
+      48           1 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      49             : 
+      50           1 :   sh_passthrough_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_);
+      51             : 
+      52             :   // | ---------------- publishers initialization --------------- |
+      53           1 :   ph_odom_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);  // needed for tf
+      54           1 :   if (ch_->debug_topics.state) {
+      55           1 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      56             :   }
+      57           1 :   if (ch_->debug_topics.covariance) {
+      58           0 :     ph_pose_covariance_  = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, Support::toSnakeCase(getName()) + "/pose_covariance", 10);
+      59           0 :     ph_twist_covariance_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, Support::toSnakeCase(getName()) + "/twist_covariance", 10);
+      60             :   }
+      61           1 :   if (ch_->debug_topics.innovation) {
+      62           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      63             :   }
+      64           1 :   if (ch_->debug_topics.diag) {
+      65           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, Support::toSnakeCase(getName()) + "/diagnostics", 10);
+      66             :   }
+      67             : 
+      68             :   // | ------------------ initialize published messages ------------------ |
+      69           1 :   uav_state_init_.header.frame_id = ns_frame_id_;
+      70           1 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+      71             : 
+      72           1 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+      73           1 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+      74           1 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+      75             : 
+      76           1 :   innovation_init_.header.frame_id         = ns_frame_id_;
+      77           1 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+      78           1 :   innovation_init_.pose.pose.orientation.w = 1.0;
+      79             : 
+      80             :   // | ------------------ finish initialization ----------------- |
+      81             : 
+      82           1 :   if (changeState(INITIALIZED_STATE)) {
+      83           1 :     ROS_INFO("[%s]: Estimator initialized", getPrintName().c_str());
+      84             :   } else {
+      85           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+      86             :   }
+      87           1 : }
+      88             : /*//}*/
+      89             : 
+      90             : /*//{ start() */
+      91           1 : bool Passthrough::start(void) {
+      92             : 
+      93             : 
+      94           1 :   if (isInState(READY_STATE)) {
+      95             : 
+      96           1 :     timer_update_.start();
+      97           1 :     changeState(STARTED_STATE);
+      98           1 :     return true;
+      99             : 
+     100             :   } else {
+     101           0 :     ROS_WARN("[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     102           0 :     ros::Duration(1.0).sleep();
+     103             :   }
+     104           0 :   return false;
+     105             : 
+     106             :   ROS_ERROR("[%s]: Failed to start", getPrintName().c_str());
+     107             :   return false;
+     108             : }
+     109             : /*//}*/
+     110             : 
+     111             : /*//{ pause() */
+     112           0 : bool Passthrough::pause(void) {
+     113             : 
+     114           0 :   if (isInState(RUNNING_STATE)) {
+     115           0 :     changeState(STOPPED_STATE);
+     116           0 :     return true;
+     117             :   }
+     118           0 :   return false;
+     119             : }
+     120             : /*//}*/
+     121             : 
+     122             : /*//{ reset() */
+     123           0 : bool Passthrough::reset(void) {
+     124             : 
+     125           0 :   if (!isInitialized()) {
+     126           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     127           0 :     return false;
+     128             :   }
+     129             : 
+     130           0 :   changeState(STOPPED_STATE);
+     131             : 
+     132           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     133             : 
+     134           0 :   return true;
+     135             : }
+     136             : /*//}*/
+     137             : 
+     138             : /* timerUpdate() //{*/
+     139        1055 : void Passthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     140             : 
+     141             : 
+     142        1055 :   if (!isInitialized()) {
+     143           0 :     return;
+     144             :   }
+     145             : 
+     146        1055 :   const ros::Time time_now = ros::Time::now();
+     147             : 
+     148        2110 :   nav_msgs::OdometryConstPtr msg = sh_passthrough_odom_.getMsg();
+     149             : 
+     150        1055 :   if (first_iter_) {
+     151           1 :     prev_msg_   = msg;
+     152           1 :     first_iter_ = false;
+     153             :   }
+     154             : 
+     155        2110 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     156             : 
+     157        1055 :   uav_state.header.stamp = time_now;
+     158             : 
+     159        1055 :   uav_state.pose.position    = msg->pose.pose.position;
+     160        1055 :   uav_state.pose.orientation = msg->pose.pose.orientation;
+     161             : 
+     162        1055 :   uav_state.velocity.linear  = Support::rotateVector(msg->twist.twist.linear, msg->pose.pose.orientation);
+     163        1055 :   uav_state.velocity.angular = msg->twist.twist.angular;
+     164             : 
+     165        2110 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     166             : 
+     167        2110 :   nav_msgs::Odometry innovation = innovation_init_;
+     168        1055 :   innovation.header.stamp       = time_now;
+     169             : 
+     170        1055 :   innovation.pose.pose.position.x = prev_msg_->pose.pose.position.x - msg->pose.pose.position.x;
+     171        1055 :   innovation.pose.pose.position.y = prev_msg_->pose.pose.position.y - msg->pose.pose.position.y;
+     172        1055 :   innovation.pose.pose.position.z = prev_msg_->pose.pose.position.z - msg->pose.pose.position.z;
+     173             : 
+     174        2110 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     175        1055 :   pose_covariance.header.stamp  = time_now;
+     176        1055 :   twist_covariance.header.stamp = time_now;
+     177             : 
+     178        1055 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     179        1055 :   pose_covariance.values.resize(n_states * n_states);
+     180        1055 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     181        1055 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     182        1055 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     183             : 
+     184        1055 :   twist_covariance.values.resize(n_states * n_states);
+     185        1055 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     186        1055 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     187        1055 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     188             : 
+     189        1055 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     190        1055 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     191        1055 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     192        1055 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     193        1055 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     194             : 
+     195        1055 :   publishUavState();
+     196        1055 :   publishOdom();
+     197        1055 :   publishCovariance();
+     198        1055 :   publishInnovation();
+     199        1055 :   publishDiagnostics();
+     200             : 
+     201        1055 :   prev_msg_ = msg;
+     202             : }
+     203             : /*//}*/
+     204             : 
+     205             : /*//{ timerCheckHealth() */
+     206        1071 : void Passthrough::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     207             : 
+     208        1071 :   if (!isInitialized()) {
+     209           0 :     return;
+     210             :   }
+     211             : 
+     212        1071 :   if (isInState(INITIALIZED_STATE)) {
+     213             : 
+     214          14 :     if (sh_passthrough_odom_.hasMsg()) {
+     215           1 :       changeState(READY_STATE);
+     216           1 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     217             :     } else {
+     218          13 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_passthrough_odom_.topicName().c_str());
+     219          13 :       return;
+     220             :     }
+     221             :   }
+     222             : 
+     223             : 
+     224        1058 :   if (isInState(STARTED_STATE)) {
+     225             : 
+     226           1 :     changeState(RUNNING_STATE);
+     227             :   }
+     228             : }
+     229             : /*//}*/
+     230             : 
+     231             : /*//{ isConverged() */
+     232           0 : bool Passthrough::isConverged() {
+     233             : 
+     234             :   // TODO: check convergence by rate of change of determinant
+     235             :   // most likely not used in top-level estimator
+     236             : 
+     237           0 :   return true;
+     238             : }
+     239             : /*//}*/
+     240             : 
+     241             : /*//{ setUavState() */
+     242           0 : bool Passthrough::setUavState([[maybe_unused]] const mrs_msgs::UavState &uav_state) {
+     243             : 
+     244           0 :   if (!isInState(STOPPED_STATE)) {
+     245           0 :     ROS_WARN("[%s]: Estimator state can be set only in the STOPPED state", getPrintName().c_str());
+     246           0 :     return false;
+     247             :   }
+     248             : 
+     249           0 :   ROS_WARN("[%s]: Setting the state of this estimator is not implemented.", getPrintName().c_str());
+     250           0 :   return false;
+     251             : }
+     252             : /*//}*/
+     253             : 
+     254             : }  // namespace passthrough
+     255             : 
+     256             : }  // namespace mrs_uav_state_estimators
+     257             : 
+     258             : #include <pluginlib/class_list_macros.h>
+     259           1 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::passthrough::Passthrough, mrs_uav_managers::StateEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html new file mode 100644 index 0000000000..5a663b9a3d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html @@ -0,0 +1,85 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/passthrough.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a57b3d1729bc3cdac5e4ae2ad4e24468373c0602 GIT binary patch literal 1099 zcmV-R1ho5!P)CR&-|8ai1Q<*+6d4na1&0Mk zrYzb^4VF?m$^_?ucXr{IX=DbBSx|OS#%OII(-i59jDZGlK*u>+0!@d@)G^qPw`1vA z5I8zu8oC^q8cqViaU^8tqMJbA8e^E|!GRmdB+OBks`;Z>FYQ>7@56w1Mg`no%WEq{K{?J={g%RO<`JEIPElp`a-e>NZIyFAu`fr1cTx;zevpp>H-w=Hez0H1or28Hphso^eiqdNx~U5^`VZgEm@}w4@WW}Lj9fl` zQh^>fSSI5s)`=AI-C#5(vSv*jj^M8h#8fwzUQBX)Ehq= z&a44HYI+|ysN>BqtwJfV;+~R5PyGnKID!Os`AF$kci}8L-2Nn&UjS;DQ8lVZUjRF- z&d7zg1vD^YxKZLm-~(f9TvTjOp5YdgLT@GLD!yc0P002ovPDHLkV1lbW1y=w7 literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html new file mode 100644 index 0000000000..0d64d2d899 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()2
mrs_uav_state_estimators::rtk::Rtk::Rtk()2
mrs_uav_state_estimators::rtk::Rtk::~Rtk()2
mrs_uav_state_estimators::rtk::Rtk::~Rtk().22
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html new file mode 100644 index 0000000000..8ea669eee7 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()2
mrs_uav_state_estimators::rtk::Rtk::Rtk()2
mrs_uav_state_estimators::rtk::Rtk::~Rtk()2
mrs_uav_state_estimators::rtk::Rtk::~Rtk().22
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html new file mode 100644 index 0000000000..5de9a9f3df --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html new file mode 100644 index 0000000000..5ce9521bbc --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_state_estimators/estimators/state/state_generic.h>
+       2             : 
+       3             : namespace mrs_uav_state_estimators
+       4             : {
+       5             : 
+       6             : namespace rtk
+       7             : {
+       8             : 
+       9             : const char estimator_name[] = "rtk";
+      10             : const bool is_core_plugin = true;
+      11             : 
+      12             : class Rtk : public StateGeneric {
+      13             : public:
+      14           2 :   Rtk() : StateGeneric(estimator_name, is_core_plugin) {
+      15           2 :   }
+      16             : 
+      17           4 :   ~Rtk(void) {
+      18           4 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace rtk
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25           2 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::rtk::Rtk, mrs_uav_managers::StateEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html new file mode 100644 index 0000000000..6e78d250c4 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html @@ -0,0 +1,27 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a73b427303050221946009b0afa810dd7c98d9b4 GIT binary patch literal 229 zcmeAS@N?(olHy`uVBq!ia0vp^0YEIt!VDxsGPYO&DTx4|5ZC|z|E~gqp;PJr literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html new file mode 100644 index 0000000000..69d649e2a9 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()2
mrs_uav_state_estimators::rtk_garmin::RtkGarmin::RtkGarmin()2
mrs_uav_state_estimators::rtk_garmin::RtkGarmin::~RtkGarmin()2
mrs_uav_state_estimators::rtk_garmin::RtkGarmin::~RtkGarmin().22
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html new file mode 100644 index 0000000000..37a3b89746 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()2
mrs_uav_state_estimators::rtk_garmin::RtkGarmin::RtkGarmin()2
mrs_uav_state_estimators::rtk_garmin::RtkGarmin::~RtkGarmin()2
mrs_uav_state_estimators::rtk_garmin::RtkGarmin::~RtkGarmin().22
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.frameset.html new file mode 100644 index 0000000000..5a40bb2c46 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html new file mode 100644 index 0000000000..cc81fd02ae --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-25 05:42:57Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_state_estimators/estimators/state/state_generic.h>
+       2             : 
+       3             : namespace mrs_uav_state_estimators
+       4             : {
+       5             : 
+       6             : namespace rtk_garmin
+       7             : {
+       8             : 
+       9             : const char estimator_name[] = "rtk_garmin";
+      10             : const bool is_core_plugin = true;
+      11             : 
+      12             : class RtkGarmin : public StateGeneric {
+      13             : public:
+      14           2 :   RtkGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+      15           2 :   }
+      16             : 
+      17           4 :   ~RtkGarmin(void) {
+      18           4 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace rtk_garmin
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25           2 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::rtk_garmin::RtkGarmin, mrs_uav_managers::StateEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.overview.html new file mode 100644 index 0000000000..36bf07081e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.overview.html @@ -0,0 +1,27 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..606fb073ca70818dc0b3b5dba84fd6a7044d0512 GIT binary patch literal 231 zcmeAS@N?(olHy`uVBq!ia0vp^0YEIt!VDxsGPYO&DTx4|5ZC|z|E~gq^JNDtZ&EXPgU2N!e?RY*!@G!k?Ta()7BTAKis#>5!bv}um4PI*Z02~7mGQc zuCNw3Xkx0Ko_*uhD$z@81X?onZt12fEV|g{c9?_v+hwVbPc627Ikr{%)|^j!tT$gZ WJ^4%e>pP$W7(8A5T-G@yGywov30pG& literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..e59bf80d2e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-04-25 05:42:57Functions:71258.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::StateGeneric::isConverged()0
mrs_uav_state_estimators::StateGeneric::setUavState(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::StateGeneric::start()7239
mrs_uav_state_estimators::StateGeneric::updateUavState()183561
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)193843
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)193843
mrs_uav_state_estimators::StateGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda()#1}::operator()() const291714
mrs_uav_state_estimators::StateGeneric::getHeading() const291716
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html new file mode 100644 index 0000000000..c91db93aaa --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-04-25 05:42:57Functions:71258.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::StateGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)99
mrs_uav_state_estimators::StateGeneric::isConverged()0
mrs_uav_state_estimators::StateGeneric::setUavState(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)193843
mrs_uav_state_estimators::StateGeneric::updateUavState()183561
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)193843
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::start()7239
mrs_uav_state_estimators::StateGeneric::getHeading() const291716
mrs_uav_state_estimators::StateGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)::{lambda()#1}::operator()() const291714
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html new file mode 100644 index 0000000000..ae1b3eba16 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html new file mode 100644 index 0000000000..1cc8ac9cc1 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html @@ -0,0 +1,633 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-04-25 05:42:57Functions:71258.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <mrs_uav_state_estimators/estimators/state/state_generic.h>
+       4             : 
+       5             : //}
+       6             : 
+       7             : namespace mrs_uav_state_estimators
+       8             : {
+       9             : 
+      10             : /* initialize() //{*/
+      11          99 : void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      12             : 
+      13          99 :   ch_ = ch;
+      14          99 :   ph_ = ph;
+      15             : 
+      16         198 :   ros::NodeHandle nh(parent_nh);
+      17             : 
+      18          99 :   if (is_core_plugin_) {
+      19             : 
+      20          99 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      21          99 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      22             :   }
+      23             : 
+      24          99 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          99 :   ph->param_loader->loadParam("estimators/lateral/name", est_lat_name_);
+      27          99 :   ph->param_loader->loadParam("estimators/altitude/name", est_alt_name_);
+      28          99 :   ph->param_loader->loadParam("estimators/heading/name", est_hdg_name_);
+      29          99 :   ph->param_loader->loadParam("estimators/heading/passthrough", is_hdg_passthrough_);
+      30             : 
+      31          99 :   ph->param_loader->loadParam("override_frame_id/enabled", is_override_frame_id_);
+      32          99 :   if (is_override_frame_id_) {
+      33           0 :     ph->param_loader->loadParam("override_frame_id/frame_id", frame_id_);
+      34             :   }
+      35             : 
+      36         198 :   std::string topic_orientation;
+      37          99 :   ph->param_loader->loadParam("topics/orientation", topic_orientation);
+      38          99 :   topic_orientation_ = "/" + ch_->uav_name + "/" + topic_orientation;
+      39         198 :   std::string topic_angular_velocity;
+      40          99 :   ph->param_loader->loadParam("topics/angular_velocity", topic_angular_velocity);
+      41          99 :   topic_angular_velocity_ = "/" + ch_->uav_name + "/" + topic_angular_velocity;
+      42             : 
+      43          99 :   if (!ph->param_loader->loadedSuccessfully()) {
+      44           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      45           0 :     ros::shutdown();
+      46             :   }
+      47             : 
+      48          99 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      49             : 
+      50             :   // | ------------------ timers initialization ----------------- |
+      51          99 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerUpdate, this);  // not running after init
+      52             :   /* timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerCheckHealth, this); */
+      53          99 :   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
+      54             : 
+      55             :   // | --------------- subscribers initialization --------------- |
+      56         198 :   mrs_lib::SubscribeHandlerOptions shopts;
+      57          99 :   shopts.nh                 = nh;
+      58          99 :   shopts.node_name          = getPrintName();
+      59          99 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      60          99 :   shopts.threadsafe         = true;
+      61          99 :   shopts.autostart          = true;
+      62          99 :   shopts.queue_size         = 10;
+      63          99 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      64             : 
+      65          99 :   sh_hw_api_orient_  = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, topic_orientation_);
+      66          99 :   sh_hw_api_ang_vel_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, topic_angular_velocity_);
+      67             : 
+      68             :   // | ---------------- publishers initialization --------------- |
+      69          99 :   ph_odom_     = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);
+      70          99 :   ph_attitude_ = mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped>(nh, Support::toSnakeCase(getName()) + "/attitude", 10);
+      71             : 
+      72          99 :   if (ch_->debug_topics.state) {
+      73          99 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      74             :   }
+      75          99 :   if (ch_->debug_topics.covariance) {
+      76           0 :     ph_pose_covariance_  = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, Support::toSnakeCase(getName()) + "/pose_covariance", 10);
+      77           0 :     ph_twist_covariance_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, Support::toSnakeCase(getName()) + "/twist_covariance", 10);
+      78             :   }
+      79          99 :   if (ch_->debug_topics.innovation) {
+      80           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      81             :   }
+      82          99 :   if (ch_->debug_topics.diag) {
+      83           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, Support::toSnakeCase(getName()) + "/diagnostics", 10);
+      84             :   }
+      85             : 
+      86             :   // | ---------------- estimators initialization --------------- |
+      87         198 :   std::vector<double> max_altitudes;
+      88             : 
+      89          99 :   if (is_hdg_passthrough_) {
+      90          99 :     est_hdg_ = std::make_unique<HdgPassthrough>(est_hdg_name_, frame_id_, getName(), is_core_plugin_);
+      91             :   } else {
+      92           0 :     est_hdg_ = std::make_unique<HdgGeneric>(est_hdg_name_, frame_id_, getName(), is_core_plugin_);
+      93             :   }
+      94          99 :   est_hdg_->initialize(nh, ch_, ph_);
+      95          99 :   max_altitudes.push_back(est_hdg_->getMaxFlightZ());
+      96             : 
+      97      291813 :   est_lat_ = std::make_unique<LatGeneric>(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); });
+      98          99 :   est_lat_->initialize(nh, ch_, ph_);
+      99          99 :   max_altitudes.push_back(est_lat_->getMaxFlightZ());
+     100             : 
+     101          99 :   est_alt_ = std::make_unique<AltGeneric>(est_alt_name_, frame_id_, getName(), is_core_plugin_);
+     102          99 :   est_alt_->initialize(nh, ch_, ph_);
+     103          99 :   max_altitudes.push_back(est_alt_->getMaxFlightZ());
+     104             : 
+     105          99 :   max_flight_z_ = *std::min_element(max_altitudes.begin(), max_altitudes.end());
+     106             : 
+     107             :   // | ------------------ initialize published messages ------------------ |
+     108          99 :   uav_state_init_.header.frame_id = ns_frame_id_;
+     109          99 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+     110             : 
+     111          99 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+     112          99 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+     113          99 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+     114             : 
+     115          99 :   innovation_init_.header.frame_id         = ns_frame_id_;
+     116          99 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+     117          99 :   innovation_init_.pose.pose.orientation.w = 1.0;
+     118             : 
+     119             :   // | ------------------ finish initialization ----------------- |
+     120             : 
+     121          99 :   if (changeState(INITIALIZED_STATE)) {
+     122          99 :     ROS_INFO("[%s]: Estimator initialized", getPrintName().c_str());
+     123             :   } else {
+     124           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+     125             :   }
+     126          99 : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ start() */
+     130        7239 : bool StateGeneric::start(void) {
+     131             : 
+     132        7239 :   if (isInState(READY_STATE)) {
+     133             : 
+     134             :     bool est_lat_start_successful, est_alt_start_successful, est_hdg_start_successful;
+     135             : 
+     136        7239 :     if (est_lat_->isStarted() || est_lat_->isRunning()) {
+     137        3309 :       est_lat_start_successful = true;
+     138             :     } else {
+     139        3930 :       est_lat_start_successful = est_lat_->start();
+     140             :     }
+     141             : 
+     142        7239 :     if (est_alt_->isStarted() || est_alt_->isRunning()) {
+     143        3235 :       est_alt_start_successful = true;
+     144             :     } else {
+     145        4004 :       est_alt_start_successful = est_alt_->start();
+     146             :     }
+     147             : 
+     148        7239 :     if (est_hdg_->isStarted() || est_hdg_->isRunning()) {
+     149        7072 :       timer_pub_attitude_.start();
+     150        7072 :       est_hdg_start_successful = true;
+     151             :     } else {
+     152         167 :       est_hdg_start_successful = est_hdg_->start();
+     153             :     }
+     154             : 
+     155        7239 :     if (est_lat_start_successful && est_alt_start_successful && est_hdg_start_successful) {
+     156             :       /* timer_update_.start(); */
+     157          99 :       changeState(STARTED_STATE);
+     158          99 :       return true;
+     159             :     }
+     160             : 
+     161             :   } else {
+     162           0 :     ROS_WARN("[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     163           0 :     ros::Duration(1.0).sleep();
+     164             :   }
+     165        7140 :   return false;
+     166             : 
+     167             :   ROS_ERROR("[%s]: Failed to start", getPrintName().c_str());
+     168             :   return false;
+     169             : }
+     170             : /*//}*/
+     171             : 
+     172             : /*//{ pause() */
+     173           0 : bool StateGeneric::pause(void) {
+     174             : 
+     175           0 :   if (isInState(RUNNING_STATE)) {
+     176           0 :     est_lat_->pause();
+     177           0 :     est_alt_->pause();
+     178           0 :     est_hdg_->pause();
+     179           0 :     changeState(STOPPED_STATE);
+     180           0 :     return true;
+     181             : 
+     182             :   } else {
+     183           0 :     return false;
+     184             :   }
+     185             : }
+     186             : /*//}*/
+     187             : 
+     188             : /*//{ reset() */
+     189           0 : bool StateGeneric::reset(void) {
+     190             : 
+     191           0 :   if (!isInitialized()) {
+     192           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     193           0 :     return false;
+     194             :   }
+     195             : 
+     196           0 :   est_lat_->pause();
+     197           0 :   est_alt_->pause();
+     198           0 :   est_hdg_->pause();
+     199           0 :   changeState(STOPPED_STATE);
+     200             : 
+     201           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     202             : 
+     203           0 :   return true;
+     204             : }
+     205             : /*//}*/
+     206             : 
+     207             : /* timerUpdate() //{*/
+     208      193843 : void StateGeneric::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     209             : 
+     210             : 
+     211      193843 :   if (!isInitialized()) {
+     212       10282 :     return;
+     213             :   }
+     214             : 
+     215      386420 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     216             : 
+     217      193210 :   switch (getCurrentSmState()) {
+     218             : 
+     219           0 :     case UNINITIALIZED_STATE: {
+     220           0 :       break;
+     221             :     }
+     222        2426 :     case INITIALIZED_STATE: {
+     223             : 
+     224        2426 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     225          99 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     226          99 :           changeState(READY_STATE);
+     227          99 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     228             :         } else {
+     229           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     230           0 :           return;
+     231             :         }
+     232             :       } else {
+     233        2327 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_hw_api_orient_.topicName().c_str());
+     234        2327 :         return;
+     235             :       }
+     236             : 
+     237          99 :       break;
+     238             :     }
+     239             : 
+     240        7195 :     case READY_STATE: {
+     241        7195 :       break;
+     242             :     }
+     243             : 
+     244         127 :     case STARTED_STATE: {
+     245             : 
+     246         127 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     247             : 
+     248         127 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     249          99 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     250          99 :         changeState(RUNNING_STATE);
+     251             :       } else {
+     252          28 :         return;
+     253             :       }
+     254          99 :       break;
+     255             :     }
+     256             : 
+     257      183462 :     case RUNNING_STATE: {
+     258      183462 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     259           0 :         changeState(ERROR_STATE);
+     260             :       }
+     261      183462 :       break;
+     262             :     }
+     263             : 
+     264           0 :     case STOPPED_STATE: {
+     265           0 :       break;
+     266             :     }
+     267             : 
+     268           0 :     case ERROR_STATE: {
+     269           0 :       if ((est_lat_->isReady() || est_lat_->isRunning()) && (est_alt_->isReady() || est_alt_->isRunning()) && (est_hdg_->isReady() || est_hdg_->isRunning())) {
+     270           0 :         changeState(READY_STATE);
+     271             :       }
+     272           0 :       break;
+     273             :     }
+     274             :   }
+     275             : 
+     276      190855 :   if (!isRunning() && !isStarted()) {
+     277        7294 :     return;
+     278             :   }
+     279             : 
+     280      183561 :   updateUavState();
+     281             : 
+     282      183560 :   publishUavState();
+     283      183561 :   publishOdom();
+     284      183560 :   publishCovariance();
+     285      183561 :   publishInnovation();
+     286      183561 :   publishDiagnostics();
+     287             : }
+     288             : /*//}*/
+     289             : 
+     290             : /*//{ timerCheckHealth() */
+     291           0 : void StateGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     292             : 
+     293           0 :   if (!isInitialized()) {
+     294           0 :     return;
+     295             :   }
+     296             : 
+     297           0 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299           0 :   switch (getCurrentSmState()) {
+     300             : 
+     301           0 :     case UNINITIALIZED_STATE: {
+     302           0 :       break;
+     303             :     }
+     304           0 :     case INITIALIZED_STATE: {
+     305             : 
+     306           0 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     307           0 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     308           0 :           changeState(READY_STATE);
+     309           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     310             :         } else {
+     311           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     312           0 :           return;
+     313             :         }
+     314             :       } else {
+     315           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_hw_api_orient_.topicName().c_str());
+     316           0 :         return;
+     317             :       }
+     318             : 
+     319           0 :       break;
+     320             :     }
+     321             : 
+     322           0 :     case READY_STATE: {
+     323           0 :       break;
+     324             :     }
+     325             : 
+     326           0 :     case STARTED_STATE: {
+     327             : 
+     328           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     329             : 
+     330           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     331           0 :         changeState(ERROR_STATE);
+     332             :       }
+     333             : 
+     334           0 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     335           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     336           0 :         changeState(RUNNING_STATE);
+     337             :       } else {
+     338           0 :         return;
+     339             :       }
+     340           0 :       break;
+     341             :     }
+     342             : 
+     343           0 :     case RUNNING_STATE: {
+     344           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     345           0 :         changeState(ERROR_STATE);
+     346             :       }
+     347           0 :       break;
+     348             :     }
+     349             : 
+     350           0 :     case STOPPED_STATE: {
+     351           0 :       break;
+     352             :     }
+     353             : 
+     354           0 :     case ERROR_STATE: {
+     355           0 :       if (est_lat_->isReady() && est_alt_->isReady() && est_hdg_->isReady()) {
+     356           0 :         changeState(READY_STATE);
+     357             :       }
+     358           0 :       break;
+     359             :     }
+     360             :   }
+     361             : }
+     362             : /*//}*/
+     363             : 
+     364             : /* timerPubAttitude() //{*/
+     365      193843 : void StateGeneric::timerPubAttitude([[maybe_unused]] const ros::TimerEvent &event) {
+     366             : 
+     367      193843 :   if (!isInitialized()) {
+     368        3238 :     return;
+     369             :   }
+     370             : 
+     371      386420 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerPubAttitude", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     372             : 
+     373      193210 :   if (!sh_hw_api_orient_.hasMsg()) {
+     374        1152 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation on topic %s yet", getPrintName().c_str(), sh_hw_api_orient_.topicName().c_str());
+     375        1152 :     return;
+     376             :   }
+     377             : 
+     378      192057 :   if (!est_hdg_->isRunning() && !isError()) {
+     379        1453 :     ROS_WARN_THROTTLE(1.0, "[%s]: cannot publish attitude, heading estimator is not running", getPrintName().c_str());
+     380        1453 :     return;
+     381             :   }
+     382             : 
+     383      190605 :   scope_timer.checkpoint("checks");
+     384             : 
+     385      190605 :   const ros::Time time_now = ros::Time::now();
+     386             : 
+     387      190605 :   geometry_msgs::QuaternionStamped att;
+     388      190605 :   att.header.stamp    = time_now;
+     389      190605 :   att.header.frame_id = ns_frame_id_ + "_att_only";
+     390             : 
+     391             :   double hdg;
+     392      190605 :   if (isError()) {
+     393           0 :     hdg = est_hdg_->getLastValidHdg();
+     394             :   } else {
+     395      190605 :     hdg = est_hdg_->getState(POSITION);
+     396             :   }
+     397             : 
+     398      190604 :   auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     399      190603 :   if (res) {
+     400      190602 :     att.quaternion = res.value();
+     401             :   } else {
+     402           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: could not rotate orientation by heading", getPrintName().c_str());
+     403           0 :     return;
+     404             :   }
+     405             : 
+     406      190602 :   scope_timer.checkpoint("rotate");
+     407             : 
+     408      190603 :   if (!Support::noNans(att.quaternion)) {
+     409           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaNs in timerPubAttitude quaternion", getPrintName().c_str());
+     410           0 :     return;
+     411             :   }
+     412             : 
+     413      190603 :   scope_timer.checkpoint("nan check");
+     414             : 
+     415      190602 :   ph_attitude_.publish(att);
+     416      190605 :   scope_timer.checkpoint("publish");
+     417             : }
+     418             : /*//}*/
+     419             : 
+     420             : /*//{ isConverged() */
+     421           0 : bool StateGeneric::isConverged() {
+     422             : 
+     423             :   // TODO: check convergence by rate of change of determinant
+     424             :   // most likely not used in top-level estimator
+     425             : 
+     426           0 :   return true;
+     427             : }
+     428             : /*//}*/
+     429             : 
+     430             : /*//{ updateUavState() */
+     431      183561 : void StateGeneric::updateUavState() {
+     432             : 
+     433      183561 :   if (!sh_hw_api_orient_.hasMsg()) {
+     434           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation on topic %s yet", getPrintName().c_str(), sh_hw_api_orient_.topicName().c_str());
+     435           0 :     return;
+     436             :   }
+     437             : 
+     438      183561 :   if (!sh_hw_api_ang_vel_.hasMsg()) {
+     439           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity on topic %s yet", getPrintName().c_str(), sh_hw_api_ang_vel_.topicName().c_str());
+     440           0 :     return;
+     441             :   }
+     442      367122 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::updateUavState", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     443             : 
+     444      183561 :   const ros::Time time_now = ros::Time::now();
+     445             : 
+     446      183561 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     447      183561 :   uav_state.header.stamp       = time_now;
+     448             : 
+     449             :   // do not rotate orientation if passthrough hdg
+     450      183561 :   if (est_hdg_name_ == "hdg_passthrough") {
+     451           0 :     uav_state.pose.orientation = sh_hw_api_orient_.getMsg()->quaternion;
+     452             :   } else {
+     453             :     double hdg;
+     454      183561 :     if (isError()) {
+     455           0 :       hdg = est_hdg_->getLastValidHdg();
+     456             :     } else {
+     457      183561 :       hdg = est_hdg_->getState(POSITION);
+     458             :     }
+     459             : 
+     460      183561 :     auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     461      183556 :     if (res) {
+     462      183561 :       uav_state.pose.orientation = res.value();
+     463             :     } else {
+     464           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not rotate orientation by heading", getPrintName().c_str());
+     465           0 :       return;
+     466             :     }
+     467             :   }
+     468             : 
+     469      183553 :   scope_timer.checkpoint("rotate orientation");
+     470             : 
+     471      183560 :   uav_state.velocity.angular = sh_hw_api_ang_vel_.getMsg()->vector;
+     472             : 
+     473      183558 :   uav_state.pose.position.x = est_lat_->getState(POSITION, AXIS_X);
+     474      183560 :   uav_state.pose.position.y = est_lat_->getState(POSITION, AXIS_Y);
+     475      183559 :   uav_state.pose.position.z = est_alt_->getState(POSITION);
+     476             : 
+     477      183558 :   uav_state.velocity.linear.x = est_lat_->getState(VELOCITY, AXIS_X);  // in global frame
+     478      183560 :   uav_state.velocity.linear.y = est_lat_->getState(VELOCITY, AXIS_Y);  // in global frame
+     479      183558 :   uav_state.velocity.linear.z = est_alt_->getState(VELOCITY);          // in global frame
+     480             : 
+     481      183555 :   uav_state.acceleration.linear.x = est_lat_->getState(ACCELERATION, AXIS_X);  // in global frame
+     482      183561 :   uav_state.acceleration.linear.y = est_lat_->getState(ACCELERATION, AXIS_Y);  // in global frame
+     483      183560 :   uav_state.acceleration.linear.z = est_alt_->getState(ACCELERATION);          // in global frame
+     484             : 
+     485      183559 :   scope_timer.checkpoint("fill uav state");
+     486             : 
+     487      367112 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     488      183561 :   scope_timer.checkpoint("uav state to odom");
+     489             : 
+     490      367113 :   nav_msgs::Odometry innovation = innovation_init_;
+     491      183558 :   innovation.header.stamp       = time_now;
+     492             : 
+     493      183558 :   innovation.pose.pose.position.x = est_lat_->getInnovation(POSITION, AXIS_X);
+     494      183559 :   innovation.pose.pose.position.y = est_lat_->getInnovation(POSITION, AXIS_Y);
+     495      183557 :   innovation.pose.pose.position.z = est_alt_->getInnovation(POSITION);
+     496             : 
+     497      183552 :   is_mitigating_jump_ = est_alt_->isMitigatingJump() || est_lat_->isMitigatingJump() || est_hdg_->isMitigatingJump();
+     498             : 
+     499      183558 :   scope_timer.checkpoint("innovation");
+     500             : 
+     501      367112 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     502      183544 :   pose_covariance.header.stamp  = time_now;
+     503      183544 :   twist_covariance.header.stamp = time_now;
+     504             : 
+     505      183544 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     506      183544 :   pose_covariance.values.resize(n_states * n_states);
+     507      183556 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(POSITION, AXIS_X);
+     508      183544 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(POSITION, AXIS_Y);
+     509      183554 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(POSITION);
+     510             : 
+     511      183550 :   twist_covariance.values.resize(n_states * n_states);
+     512      183552 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(VELOCITY, AXIS_X);
+     513      183556 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(VELOCITY, AXIS_Y);
+     514      183560 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(VELOCITY);
+     515             : 
+     516      183559 :   scope_timer.checkpoint("covariance");
+     517             : 
+     518      183552 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     519      183552 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     520      183554 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     521      183557 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     522      183550 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     523             : }
+     524             : /*//}*/
+     525             : 
+     526             : /*//{ getHeading() */
+     527      291716 : std::optional<double> StateGeneric::getHeading() const {
+     528      291716 :   if (!est_hdg_->isRunning()) {
+     529         212 :     return {};
+     530             :   }
+     531      291507 :   return est_hdg_->getState(POSITION);
+     532             : }
+     533             : /*//}*/
+     534             : 
+     535             : /*//{ setUavState() */
+     536           0 : bool StateGeneric::setUavState([[maybe_unused]] const mrs_msgs::UavState &uav_state) {
+     537             : 
+     538           0 :   if (!isInState(STOPPED_STATE)) {
+     539           0 :     ROS_WARN("[%s]: Estimator state can be set only in the STOPPED state", getPrintName().c_str());
+     540           0 :     return false;
+     541             :   }
+     542             : 
+     543           0 :   ROS_WARN("[%s]: Setting the state of this estimator is not implemented.", getPrintName().c_str());
+     544           0 :   return false;
+     545             : }
+     546             : /*//}*/
+     547             : 
+     548             : }  // namespace mrs_uav_state_estimators
+     549             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..200fa9f980 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html @@ -0,0 +1,158 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8938884936a7531808d51a4f305de8fd22b967fe GIT binary patch literal 1968 zcmV;h2T%BkP)RelK&I9M6mH^bcPZ98fHBq4&s^YMH>$M?VC zs`18mAeBcm#_=-d2#+Je(&dx~tD86V3hsGUb2_xUYWK-7fn%6vt!I5HAWoXHKhIZS z^y*O;@i@)3p_EZL3JB=%APo#{7vvR{GGYU^OucB#X6bp-hKPo}BiJCb8Oa^}G{}&%$F!$v*3iBPY${N5+&);VP^9#MWh` zr-XTstCuDghI5|x9;#hA+Xajk&XFw@_eP#}%iHa!d_-xG7fZT;YPDk59K| zH)9-$k+e|G=m-$kqpTgTn{F@c*vKrI2D=5hbj;Vc1a3P@n{-tz|h1I2Jd!5+_-dm%w)p76 z;PyJ!Gb;&?%?SxCDW)Oz5?SGJhC7S^v>#`@k^+T+D*zbdE)Hvb3U3DaxCBQI&ewM>X<}JsFs^_279i$dJL;q*>ylLzFyMsrjNKlxSJOU?;q%>Jun7SHKeLoRwO#@DGJbk}b z*)rf57LIX~EJ~fCg0UxH#UB?_;1tmC@B@ahb-fSBtZmO+fzvR5H) zK&VLNDq!T0I$K5?(hRP|BSqj_8_HrM7?EX#2`MzuyhuZXv^G3oJ=iapZFb+qYJi;3 z>JV3_{YP1QT#py`QVeAPW0%F5)j}~wP2a&|HIQN>T0p8Ww-nu=)fDy$O#l^R%uGVE zK`akgoYznjpo51`zu^?b7}PM-F3LnRq&geG7`5D~9+EU`1^d-W^8}AY77dSNMYGZp zcjb$Ut4+9Eub#`drh5N&4lUh`|As=dg89ttD-f^&V;S}oU^%VWQ?IkM~|} z+f5Eg3RmKhBJgdM)nX$Uw}R=)7{6?VlxLCOeDG2OEH-3Ud=~ylm2x|e^=c0zy<~mD zPpgeN1&omdDe{`m?{(==#5K7Y@%T-uI(fX_IeMEW`nsP(h0@Q{H+J#i^#Nu7* zVJGdJju2st9BO7S^mo|WnOq_bjvZsz7#g-yeTFgeo;;WIc#$s1LNPkuwv48mzCa$$ zwjCf&Iq#{prT6p^FR6vl43$Ubrd>Rh1wHm~GgUu@bZp31=)OJoovh`kFs-|gU7>0Z zSNhWR$jj^N&ZbL`>6%z2o0xLy38e}-e)>J?(Iymps2)0CKpFTeI-0euQgKI93NH;C z)ApL{;Salx=GR=J{f`fOpy6RRN}9`gsefYIPUtlkTg_=jD&Nuo-3+eH(uKV}c22t= z>)ipM{fKBP%<97Zx^)ny%hsZ9C6{72izmb;clarb5hXqBK_MJzauFh(ao*Otyrzox z<7UrcS`&hm6kP62V7YfJ#F27u0J~x49y?O*b=!haFZZl^upJugn^Mt-pZi$Aa?b+R zC-=QKy7|bRe{_0;#*kHa#+6QDj2%-^jF$jvO8T7CS=wrkHdg_f1EU7;9FMv5VaQ(z zX!tzMqoQWxfDITm!I#OI>_I>NLzf*sBTe*0NGHvLh#RQ~aiy)z8s|#D`81EoG`Q{o zUeZbAa~M5eaj6so7EG8^p6bC8lQKpyZL6yDlu3I%ys4cWFLHsT|qrREi>{=;J@R==gg z{(i%eJhR8jwpje$tJVMK0J3;%muobOdFVN{(D)CEfGpm?B=2Sb0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
joy_tracker.cpp +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
<unnamed>46.5 %66 / 14238.9 %7 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/joy_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..cc8a0c0180 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
joy_tracker.cpp +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
<unnamed>46.5 %66 / 14238.9 %7 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/index-detail.html b/mrs_uav_trackers/src/joy_tracker/index-detail.html new file mode 100644 index 0000000000..69633ada75 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
joy_tracker.cpp +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
<unnamed>46.5 %66 / 14238.9 %7 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/index-sort-f.html b/mrs_uav_trackers/src/joy_tracker/index-sort-f.html new file mode 100644 index 0000000000..0af9a817ed --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
joy_tracker.cpp +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/index-sort-l.html b/mrs_uav_trackers/src/joy_tracker/index-sort-l.html new file mode 100644 index 0000000000..78a1432e78 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
joy_tracker.cpp +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/index.html b/mrs_uav_trackers/src/joy_tracker/index.html new file mode 100644 index 0000000000..975f9e9ad6 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
joy_tracker.cpp +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..7de79dcf88 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_tracker - joy_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::joy_tracker::JoyTracker::resetStatic()0
mrs_uav_trackers::joy_tracker::JoyTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::getStatus()0
mrs_uav_trackers::joy_tracker::JoyTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()20
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::joy_tracker::JoyTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::joy_tracker::JoyTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html new file mode 100644 index 0000000000..8266bbf403 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_tracker - joy_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()20
mrs_uav_trackers::joy_tracker::JoyTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::joy_tracker::JoyTracker::resetStatic()0
mrs_uav_trackers::joy_tracker::JoyTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::joy_tracker::JoyTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::joy_tracker::JoyTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
mrs_uav_trackers::joy_tracker::JoyTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::getStatus()0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.frameset.html b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.frameset.html new file mode 100644 index 0000000000..8092e9ce7e --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html new file mode 100644 index 0000000000..8eeb2be12c --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html @@ -0,0 +1,588 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_tracker - joy_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-25 05:42:57Functions:71838.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : 
+       8             : #include <nav_msgs/Odometry.h>
+       9             : #include <sensor_msgs/Joy.h>
+      10             : 
+      11             : #include <mrs_lib/profiler.h>
+      12             : #include <mrs_lib/mutex.h>
+      13             : #include <mrs_lib/attitude_converter.h>
+      14             : #include <mrs_lib/subscribe_handler.h>
+      15             : #include <mrs_lib/geometry/cyclic.h>
+      16             : 
+      17             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      18             : 
+      19             : //}
+      20             : 
+      21             : /* defines //{ */
+      22             : 
+      23             : #define STOP_THR 1e-3
+      24             : 
+      25             : //}
+      26             : 
+      27             : /* using //{ */
+      28             : 
+      29             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      30             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      31             : 
+      32             : using radians  = mrs_lib::geometry::radians;
+      33             : using sradians = mrs_lib::geometry::sradians;
+      34             : 
+      35             : //}
+      36             : 
+      37             : namespace mrs_uav_trackers
+      38             : {
+      39             : 
+      40             : namespace joy_tracker
+      41             : {
+      42             : 
+      43             : /* //{ class JoyTracker */
+      44             : 
+      45             : class JoyTracker : public mrs_uav_managers::Tracker {
+      46             : public:
+      47             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      48             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      49             : 
+      50             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd);
+      51             :   void                          deactivate(void);
+      52             :   bool                          resetStatic(void);
+      53             : 
+      54             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState &uav_state, const mrs_uav_managers::Controller::ControlOutput &last_control_output);
+      55             :   const mrs_msgs::TrackerStatus             getStatus();
+      56             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd);
+      57             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      58             : 
+      59             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd);
+      60             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd);
+      61             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd);
+      62             : 
+      63             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      64             : 
+      65             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      66             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      67             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      68             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      69             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      70             : 
+      71             : private:
+      72             :   ros::NodeHandle nh_;
+      73             : 
+      74             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      75             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      76             : 
+      77             :   bool callbacks_enabled_ = true;
+      78             : 
+      79             :   std::string _uav_name_;
+      80             : 
+      81             :   bool is_initialized_ = false;
+      82             :   bool is_active_      = false;
+      83             : 
+      84             :   ros::Time last_update;
+      85             : 
+      86             :   // | ------------------------ uav state ----------------------- |
+      87             : 
+      88             :   mrs_msgs::UavState uav_state_;
+      89             :   bool               got_uav_state_ = false;
+      90             :   std::mutex         mutex_uav_state_;
+      91             : 
+      92             :   // | ------------------ dynamics constraints ------------------ |
+      93             : 
+      94             :   double     _heading_rate_;
+      95             :   std::mutex mutex_constraints_;
+      96             : 
+      97             :   // | ------------------ tracker's inner state ----------------- |
+      98             : 
+      99             :   double     state_z_;
+     100             :   double     state_heading_;
+     101             :   std::mutex mutex_state_;
+     102             : 
+     103             :   // | ------------------- joystick subscriber ------------------ |
+     104             : 
+     105             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     106             : 
+     107             :   double _max_tilt_;
+     108             :   double _vertical_speed_;
+     109             : 
+     110             :   // channel numbers and channel multipliers
+     111             :   int    _channel_pitch_;
+     112             :   int    _channel_roll_;
+     113             :   int    _channel_heading_;
+     114             :   int    _channel_throttle_;
+     115             :   double _channel_mult_pitch_;
+     116             :   double _channel_mult_roll_;
+     117             :   double _channel_mult_heading_;
+     118             :   double _channel_mult_throttle_;
+     119             : 
+     120             :   // | ------------------------ profiler ------------------------ |
+     121             : 
+     122             :   mrs_lib::Profiler profiler_;
+     123             :   bool              _profiler_enabled_ = false;
+     124             : };
+     125             : 
+     126             : //}
+     127             : 
+     128             : // | -------------- tracker's interface routines -------------- |
+     129             : 
+     130             : /* //{ initialize() */
+     131             : 
+     132          94 : bool JoyTracker::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     133             :                             std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     134             : 
+     135          94 :   this->common_handlers_  = common_handlers;
+     136          94 :   this->private_handlers_ = private_handlers;
+     137             : 
+     138          94 :   _uav_name_ = common_handlers->uav_name;
+     139             : 
+     140          94 :   nh_ = nh;
+     141             : 
+     142          94 :   ros::Time::waitForValid();
+     143             : 
+     144             :   // --------------------------------------------------------------
+     145             :   // |                     loading parameters                     |
+     146             :   // --------------------------------------------------------------
+     147             : 
+     148             :   // | ---------- loading params using the parent's nh ---------- |
+     149             : 
+     150         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     151             : 
+     152          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     155           0 :     ROS_ERROR("[JoyTracker]: Could not load all parameters!");
+     156           0 :     return false;
+     157             :   }
+     158             : 
+     159             :   // | ---------------- load plugin's parameters ---------------- |
+     160             : 
+     161          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/joy_tracker.yaml");
+     162          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/joy_tracker.yaml");
+     163             : 
+     164         188 :   const std::string yaml_prefix = "mrs_uav_trackers/joy_tracker/";
+     165             : 
+     166          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     167             : 
+     168          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_tilt", _max_tilt_);
+     169             : 
+     170          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     171             : 
+     172             :   // load channels
+     173          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/pitch", _channel_pitch_);
+     174          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/roll", _channel_roll_);
+     175          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/heading", _channel_heading_);
+     176          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/throttle", _channel_throttle_);
+     177             : 
+     178             :   // load channel multipliers
+     179          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/pitch", _channel_mult_pitch_);
+     180          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/roll", _channel_mult_roll_);
+     181          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/heading", _channel_mult_heading_);
+     182          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/throttle", _channel_mult_throttle_);
+     183             : 
+     184          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     185           0 :     ROS_ERROR("[JoyTracker]: could not load all parameters!");
+     186           0 :     return false;
+     187             :   }
+     188             : 
+     189             :   // | ------------------------ profiler ------------------------ |
+     190             : 
+     191          94 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "JoyTracker", _profiler_enabled_);
+     192             : 
+     193             :   // | ----------------------- subscribers ---------------------- |
+     194             : 
+     195          94 :   mrs_lib::SubscribeHandlerOptions shopts;
+     196          94 :   shopts.nh              = nh_;
+     197          94 :   shopts.node_name       = "JoyTracker";
+     198          94 :   shopts.queue_size      = 1;
+     199          94 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     200             : 
+     201          94 :   sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick");
+     202             : 
+     203             :   // | --------------------- finish the init -------------------- |
+     204             : 
+     205          94 :   last_update = ros::Time(0);
+     206             : 
+     207          94 :   is_initialized_ = true;
+     208             : 
+     209          94 :   ROS_INFO("[JoyTracker]: initialized");
+     210             : 
+     211          94 :   return true;
+     212             : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* //{ activate() */
+     217             : 
+     218           0 : std::tuple<bool, std::string> JoyTracker::activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     219             : 
+     220           0 :   std::stringstream ss;
+     221             : 
+     222           0 :   if (!got_uav_state_) {
+     223             : 
+     224           0 :     ss << "odometry not set";
+     225           0 :     return std::tuple(false, ss.str());
+     226             :   }
+     227             : 
+     228           0 :   if (!sh_joystick_.hasMsg()) {
+     229             : 
+     230           0 :     ss << "missing joystick goal";
+     231           0 :     return std::tuple(false, ss.str());
+     232             :   }
+     233             : 
+     234           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     235             : 
+     236           0 :   double uav_heading = 0;
+     237             : 
+     238             :   try {
+     239           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     240             :   }
+     241           0 :   catch (...) {
+     242           0 :     ROS_ERROR_THROTTLE(1.0, "[JoyTracker]: could not calculate UAV heading");
+     243             :   }
+     244             : 
+     245             :   // initialized the heading and z from the last tracker command / odometry
+     246             :   {
+     247           0 :     std::scoped_lock lock(mutex_state_);
+     248             : 
+     249           0 :     if (last_tracker_cmd) {
+     250             : 
+     251             :       // the last command is usable
+     252           0 :       state_z_       = last_tracker_cmd->position.z;
+     253           0 :       state_heading_ = last_tracker_cmd->heading;
+     254             : 
+     255             :     } else {
+     256             : 
+     257           0 :       state_z_       = uav_state.pose.position.z;
+     258           0 :       state_heading_ = uav_heading;
+     259             : 
+     260           0 :       ROS_WARN("[JoyTracker]: the previous command is not usable for activation, using Odometry instead");
+     261             :     }
+     262             :   }
+     263             : 
+     264           0 :   is_active_ = true;
+     265             : 
+     266           0 :   ss << "activated";
+     267           0 :   ROS_INFO_STREAM("[JoyTracker]: " << ss.str());
+     268             : 
+     269           0 :   return std::tuple(true, ss.str());
+     270             : }
+     271             : 
+     272             : //}
+     273             : 
+     274             : /* //{ deactivate() */
+     275             : 
+     276          20 : void JoyTracker::deactivate(void) {
+     277             : 
+     278          20 :   is_active_ = false;
+     279             : 
+     280          20 :   ROS_INFO("[JoyTracker]: deactivated");
+     281          20 : }
+     282             : 
+     283             : //}
+     284             : 
+     285             : /* //{ resetStatic() */
+     286             : 
+     287           0 : bool JoyTracker::resetStatic(void) {
+     288             : 
+     289           0 :   return false;
+     290             : }
+     291             : 
+     292             : //}
+     293             : 
+     294             : /* //{ update() */
+     295             : 
+     296      127134 : std::optional<mrs_msgs::TrackerCommand> JoyTracker::update(const mrs_msgs::UavState &                                          uav_state,
+     297             :                                                            [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput &last_control_output) {
+     298             : 
+     299      381402 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     300      381402 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("JoyTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     301             : 
+     302             :   {
+     303      127134 :     std::scoped_lock lock(mutex_uav_state_);
+     304             : 
+     305      127134 :     uav_state_ = uav_state;
+     306             : 
+     307      127134 :     got_uav_state_ = true;
+     308             :   }
+     309             : 
+     310      127134 :   double dt = (ros::Time::now() - last_update).toSec();
+     311             : 
+     312      127134 :   last_update = ros::Time::now();
+     313             : 
+     314             :   // up to this part the update() method is evaluated even when the tracker is not active
+     315      127134 :   if (!is_active_) {
+     316      127134 :     return {};
+     317             :   }
+     318             : 
+     319           0 :   if (!sh_joystick_.hasMsg()) {
+     320           0 :     return {};
+     321             :   }
+     322             : 
+     323             :   // | ------------------ get the joystick data ----------------- |
+     324             : 
+     325           0 :   sensor_msgs::JoyConstPtr joy_data = sh_joystick_.getMsg();
+     326             : 
+     327           0 :   double desired_vertical_speed = _channel_mult_throttle_ * joy_data->axes[_channel_throttle_] * _vertical_speed_;
+     328           0 :   double desired_heading_rate   = _channel_mult_heading_ * joy_data->axes[_channel_heading_] * _heading_rate_;
+     329           0 :   double desired_pitch          = _channel_mult_pitch_ * joy_data->axes[_channel_pitch_] * _max_tilt_;
+     330           0 :   double desired_roll           = _channel_mult_roll_ * joy_data->axes[_channel_roll_] * _max_tilt_;
+     331             : 
+     332             :   // | ----------------------- z tracking ----------------------- |
+     333             : 
+     334           0 :   state_z_ += desired_vertical_speed * dt;
+     335             : 
+     336             :   // | -------------------- heading tracking -------------------- |
+     337             : 
+     338           0 :   state_heading_ += desired_heading_rate * dt;
+     339           0 :   state_heading_ = radians::wrap(state_heading_);
+     340             : 
+     341           0 :   ROS_INFO_THROTTLE(1.0, "[JoyTracker]: desired vert_speed: %.2f, heading_speed: %.2f, pitch: %.2f, roll: %.2f", desired_vertical_speed, desired_heading_rate,
+     342             :                     desired_pitch, desired_roll);
+     343             : 
+     344           0 :   mrs_msgs::TrackerCommand tracker_cmd;
+     345             : 
+     346           0 :   tracker_cmd.header.stamp    = ros::Time::now();
+     347           0 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     348             : 
+     349           0 :   tracker_cmd.use_position_vertical = true;
+     350           0 :   tracker_cmd.position.z            = state_z_;
+     351             : 
+     352             :   // filling these anyway to allow visualization of the reference
+     353           0 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     354           0 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     355             : 
+     356           0 :   tracker_cmd.use_velocity_vertical = true;
+     357           0 :   tracker_cmd.velocity.z            = desired_vertical_speed;
+     358             : 
+     359           0 :   tracker_cmd.use_heading_rate = 1;
+     360           0 :   tracker_cmd.heading_rate     = desired_heading_rate;
+     361             : 
+     362             :   /* tracker_cmd.orientation     = mrs_lib::AttitudeConverter(desired_roll, desired_pitch, 0).setHeadingByYaw(state_heading_); */
+     363           0 :   tracker_cmd.orientation     = mrs_lib::AttitudeConverter(desired_roll, desired_pitch, state_heading_);
+     364           0 :   tracker_cmd.use_orientation = true;
+     365             : 
+     366           0 :   return {tracker_cmd};
+     367             : }
+     368             : 
+     369             : //}
+     370             : 
+     371             : /* //{ getStatus() */
+     372             : 
+     373           0 : const mrs_msgs::TrackerStatus JoyTracker::getStatus() {
+     374             : 
+     375           0 :   mrs_msgs::TrackerStatus tracker_status;
+     376             : 
+     377           0 :   tracker_status.active            = is_active_;
+     378           0 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     379             : 
+     380           0 :   return tracker_status;
+     381             : }
+     382             : 
+     383             : //}
+     384             : 
+     385             : /* //{ enableCallbacks() */
+     386             : 
+     387         297 : const std_srvs::SetBoolResponse::ConstPtr JoyTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     388             : 
+     389         594 :   std_srvs::SetBoolResponse res;
+     390         594 :   std::stringstream         ss;
+     391             : 
+     392         297 :   if (cmd->data != callbacks_enabled_) {
+     393             : 
+     394          20 :     callbacks_enabled_ = cmd->data;
+     395             : 
+     396          20 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     397          20 :     ROS_INFO_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     398             : 
+     399             :   } else {
+     400             : 
+     401         277 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     402         277 :     ROS_WARN_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     403             :   }
+     404             : 
+     405         297 :   res.message = ss.str();
+     406         297 :   res.success = true;
+     407             : 
+     408         594 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     409             : }
+     410             : 
+     411             : //}
+     412             : 
+     413             : /* switchOdometrySource() //{ */
+     414             : 
+     415           0 : const std_srvs::TriggerResponse::ConstPtr JoyTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     416           0 :   return std_srvs::TriggerResponse::Ptr();
+     417             : }
+     418             : 
+     419             : //}
+     420             : 
+     421             : /* //{ hover() */
+     422             : 
+     423           0 : const std_srvs::TriggerResponse::ConstPtr JoyTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     424             : 
+     425           0 :   return std_srvs::TriggerResponse::Ptr();
+     426             : }
+     427             : 
+     428             : //}
+     429             : 
+     430             : /* //{ startTrajectoryTracking() */
+     431             : 
+     432           0 : const std_srvs::TriggerResponse::ConstPtr JoyTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     433           0 :   return std_srvs::TriggerResponse::Ptr();
+     434             : }
+     435             : 
+     436             : //}
+     437             : 
+     438             : /* //{ stopTrajectoryTracking() */
+     439             : 
+     440           0 : const std_srvs::TriggerResponse::ConstPtr JoyTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     441           0 :   return std_srvs::TriggerResponse::Ptr();
+     442             : }
+     443             : 
+     444             : //}
+     445             : 
+     446             : /* //{ resumeTrajectoryTracking() */
+     447             : 
+     448           0 : const std_srvs::TriggerResponse::ConstPtr JoyTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     449           0 :   return std_srvs::TriggerResponse::Ptr();
+     450             : }
+     451             : 
+     452             : //}
+     453             : 
+     454             : /* //{ gotoTrajectoryStart() */
+     455             : 
+     456           0 : const std_srvs::TriggerResponse::ConstPtr JoyTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     457           0 :   return std_srvs::TriggerResponse::Ptr();
+     458             : }
+     459             : 
+     460             : //}
+     461             : 
+     462             : /* //{ setConstraints() */
+     463             : 
+     464         325 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr JoyTracker::setConstraints([
+     465             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     466             : 
+     467         325 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     468             : }
+     469             : 
+     470             : //}
+     471             : 
+     472             : /* //{ setReference() */
+     473             : 
+     474           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr JoyTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     475             : 
+     476           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     477             : }
+     478             : 
+     479             : //}
+     480             : 
+     481             : /* //{ setVelocityReference() */
+     482             : 
+     483           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr JoyTracker::setVelocityReference([
+     484             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     485           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     486             : }
+     487             : 
+     488             : //}
+     489             : 
+     490             : /* //{ setTrajectoryReference() */
+     491             : 
+     492           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr JoyTracker::setTrajectoryReference([
+     493             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     494           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     495             : }
+     496             : 
+     497             : //}
+     498             : 
+     499             : }  // namespace joy_tracker
+     500             : 
+     501             : }  // namespace mrs_uav_trackers
+     502             : 
+     503             : #include <pluginlib/class_list_macros.h>
+     504          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::joy_tracker::JoyTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..e3fb54a601 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html @@ -0,0 +1,146 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.png b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b07281049e6b3ff0652e5eed36bda1c0faa868e7 GIT binary patch literal 1740 zcmV;-1~d7IP)QIt;}fm==}}m=>S|rvFv~_$T=*+2ZUWiSx2EnYmnjEDOnI-!!Xvp7r@> zk{T!9O**$I1F#8Yi*`fHGE#wqQZtMdLlBV8SfR}Tsf=Xv=sFIP4#z;ssBR^d9Pzyd zZ&E4FXRH=AdNM|<_KGC~nWD972CthKIoS`)j0>OES`n8FL^rq}5Uor^-Wtd-T2rpc z)@ZtQ45UtCNM<^`Ci!iK7gPt!8Z#6UpUJHeYdOXXzyarHEYwq^s5UxkgaKE_jA-VN z?ci+_=rmv%lQKF6U`-KtgHf?j0n(}xs*VvOXR((3h(4z{{Qf5isHngTNdRiABi(2! zItXC+l)_jUR=qY*^Q@lWs?_7fwKIlv1EDiI9q%;W1fGZnkRjTKl>pn#i0Y23O~ZMR zueHCn)uQ?DRG9EA-1Crn5R#4xCV!_Y2t7XIr4*&t#F8YYF*ZieXcmWzAp;p!v(?En zg1}XNSqx5LI;k2ut$&wFPk<_W>XVa%(x#K3L)g0Wx!?cd}4W5(u& z0iNFQ?HG?7@BhZk0uL6xGd z0+Eb+&1inEKfZK2>#1S|6c`QAy{!P(3s^3xkd8|laR5jhjxj?9-OxWX%5Gn&;hAya zQ*#f~>YKAx!S>xgT%31d?!0(3 zWcyxIm(Dm*ldr8(jw7`N4Z0t<+Y9Aj0LG+_b9K_!zZ?(MJ@%euw`f(kcin%Lt7xhD zF&Gn~qF^=Ek6?AxyvYa(o1L}WaVQzL8Eej$IL1@a&T0gV)#d1TO#@>l2{V&7CZ{7d zrr>nW6ygx~nQb@kZHQlYE?S5`@RZHwEp8h_wA!ZdHe-}yc6;53=&=6sToy>oD2$Zy zTqX73et4mv?AdFs*QO7Y*W_ZDEc(GI6nm-?m%QI%xyfEL?fdnM){Nl8?wLLr?QXag zRS&ZzTuK)Wb``v|q_SBgHssTYGtMCV&UsV=4o6&GqfyA7`o5BTb z?Sjxjvbg!Nc2dY?JQs~NRiW|+8p<8$+QRs%7sJ+Pd~%bDzT<&ljJU%YE$N;22>J2R zT~}@W>rN2*_RZq0X57yqtBJ;-_B!H&|A>Ic27$2PhQM3Ik;g&c5dm?R%ISvj7IHYY z8*=y_*_!oZmyZ5eWn?qecQZ#_%PHT(jdXD^9xdtdbtU-`x_!VHp%eFTtp;p`jiDZ6 zz91Y^_h9egEtPT6jx=Kq0-BGGtmUkDob3||X0yN-vi4!0oWpA!HUmi2anwDD_-@g~ zeY-e%py3gaDjGkRVjgZz#`QQl+I+w!2gScO<2Iehc28j`W8jOL3b`h3!p9i+-rDc* zm@ntsE=EISL$@m0E{1G~_Z1&i;H#oXEP$d8)HQy|2c3_^tmP&NonN0-e&s}*F4{DD zpnuG^7ai?hqlsiOE~_O357n%&6N^QoS#g)~MT>8TLt}3JR^BR z + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
landoff_tracker.cpp +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
<unnamed>73.1 %433 / 59267.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..65951655db --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
landoff_tracker.cpp +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
<unnamed>73.1 %433 / 59267.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/index-detail.html b/mrs_uav_trackers/src/landoff_tracker/index-detail.html new file mode 100644 index 0000000000..5fd8de7ca8 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
landoff_tracker.cpp +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
<unnamed>73.1 %433 / 59267.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/index-sort-f.html b/mrs_uav_trackers/src/landoff_tracker/index-sort-f.html new file mode 100644 index 0000000000..daaa0b3b8a --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
landoff_tracker.cpp +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/index-sort-l.html b/mrs_uav_trackers/src/landoff_tracker/index-sort-l.html new file mode 100644 index 0000000000..be84d66a9e --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
landoff_tracker.cpp +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/index.html b/mrs_uav_trackers/src/landoff_tracker/index.html new file mode 100644 index 0000000000..c136ecbcab --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
landoff_tracker.cpp +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..6b0eeb2f42 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::landoff_tracker::LandoffTracker::resetStatic()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVertical()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)5
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)8
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)20
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)45
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()61
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::landoff_tracker::LandoffTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)146
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)179
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)219
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()634
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()634
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()2213
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4480
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()15863
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()20343
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)23295
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html new file mode 100644 index 0000000000..90ab79fd27 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()61
mrs_uav_trackers::landoff_tracker::LandoffTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)146
mrs_uav_trackers::landoff_tracker::LandoffTracker::resetStatic()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)5
mrs_uav_trackers::landoff_tracker::LandoffTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVertical()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)8
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()20343
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)20
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()15863
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4480
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()634
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)219
mrs_uav_trackers::landoff_tracker::LandoffTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()634
mrs_uav_trackers::landoff_tracker::LandoffTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)179
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)45
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()2213
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)23295
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.frameset.html b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.frameset.html new file mode 100644 index 0000000000..469c4b6023 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html new file mode 100644 index 0000000000..c17e46f8ad --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html @@ -0,0 +1,1636 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-25 05:42:57Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : 
+       8             : #include <mrs_msgs/Vec1.h>
+       9             : #include <mrs_msgs/UavState.h>
+      10             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      11             : 
+      12             : #include <mrs_lib/profiler.h>
+      13             : #include <mrs_lib/mutex.h>
+      14             : #include <mrs_lib/attitude_converter.h>
+      15             : #include <mrs_lib/utils.h>
+      16             : #include <mrs_lib/geometry/cyclic.h>
+      17             : #include <mrs_lib/geometry/misc.h>
+      18             : 
+      19             : //}
+      20             : 
+      21             : /* defines //{ */
+      22             : 
+      23             : #define STOP_THR 1e-3
+      24             : 
+      25             : //}
+      26             : 
+      27             : /* using //{ */
+      28             : 
+      29             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      30             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      31             : 
+      32             : using radians  = mrs_lib::geometry::radians;
+      33             : using sradians = mrs_lib::geometry::sradians;
+      34             : 
+      35             : //}
+      36             : 
+      37             : namespace mrs_uav_trackers
+      38             : {
+      39             : 
+      40             : namespace landoff_tracker
+      41             : {
+      42             : 
+      43             : /* //{ class LandoffTracker */
+      44             : 
+      45             : // state machine
+      46             : typedef enum
+      47             : {
+      48             : 
+      49             :   IDLE_STATE,
+      50             :   LANDED_STATE,
+      51             :   STOP_MOTION_STATE,
+      52             :   HOVER_STATE,
+      53             :   ACCELERATING_STATE,
+      54             :   DECELERATING_STATE,
+      55             :   STOPPING_STATE,
+      56             : 
+      57             : } States_t;
+      58             : 
+      59             : const char* state_names[7] = {
+      60             : 
+      61             :     "IDLING", "LANDED", "STOPPING_MOTION", "HOVERING", "ACCELERATING", "DECELERATING", "STOPPING"};
+      62             : 
+      63             : class LandoffTracker : public mrs_uav_managers::Tracker {
+      64             : public:
+      65             :   bool initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      66             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      67             : 
+      68             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd);
+      69             :   void                          deactivate(void);
+      70             :   bool                          resetStatic(void);
+      71             : 
+      72             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState& uav_state, const mrs_uav_managers::Controller::ControlOutput& last_control_output);
+      73             :   const mrs_msgs::TrackerStatus             getStatus();
+      74             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd);
+      75             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState& new_uav_state);
+      76             : 
+      77             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd);
+      78             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd);
+      79             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd);
+      80             : 
+      81             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      82             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      83             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      84             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      85             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      86             : 
+      87             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd);
+      88             : 
+      89             : private:
+      90             :   bool callbacks_enabled_ = true;
+      91             : 
+      92             :   mrs_uav_managers::Controller::ControlOutput last_control_output_;
+      93             :   std::mutex                                  mutex_last_control_output_;
+      94             : 
+      95             :   ros::NodeHandle nh_;
+      96             :   std::string     _uav_name_;
+      97             : 
+      98             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      99             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+     100             : 
+     101             :   // main timer
+     102             :   void       timerMain(const ros::TimerEvent& event);
+     103             :   ros::Timer timer_main_;
+     104             :   std::mutex mutex_main_timer_;
+     105             : 
+     106             :   // | ------------------------ uav state ----------------------- |
+     107             : 
+     108             :   mrs_msgs::UavState uav_state_;
+     109             :   bool               got_uav_state_ = false;
+     110             :   std::mutex         mutex_uav_state_;
+     111             : 
+     112             :   // | ---------------- the tracker's inner state --------------- |
+     113             : 
+     114             :   int    _main_timer_rate_;
+     115             :   double _landing_reference_;
+     116             :   double _tracker_dt_;
+     117             :   bool   is_initialized_ = false;
+     118             :   bool   is_active_      = false;
+     119             : 
+     120             :   bool   _takeoff_disable_lateral_gains_ = false;
+     121             :   double _takeoff_disable_lateral_gains_z_;
+     122             : 
+     123             :   // | --------------- the tracker's state machine -------------- |
+     124             : 
+     125             :   States_t current_state_vertical_    = IDLE_STATE;
+     126             :   States_t previous_state_vertical_   = IDLE_STATE;
+     127             :   States_t current_state_horizontal_  = IDLE_STATE;
+     128             :   States_t previous_state_horizontal_ = IDLE_STATE;
+     129             : 
+     130             :   void changeStateHorizontal(States_t new_state);
+     131             :   void changeStateVertical(States_t new_state);
+     132             :   void changeState(States_t new_state);
+     133             : 
+     134             :   std::atomic<bool> taking_off_ = false;
+     135             :   std::atomic<bool> landing_    = false;
+     136             :   std::atomic<bool> elanding_   = false;
+     137             : 
+     138             :   std::atomic<bool> cause_failsafe_ = false;
+     139             : 
+     140             :   void stopHorizontalMotion(void);
+     141             :   void stopVerticalMotion(void);
+     142             :   void accelerateVertical(void);
+     143             :   void decelerateVertical(void);
+     144             :   void stopHorizontal(void);
+     145             :   void stopVertical(void);
+     146             : 
+     147             :   // | --------------- takeoff / landing services --------------- |
+     148             : 
+     149             :   ros::ServiceServer service_takeoff_;
+     150             :   ros::ServiceServer service_land_;
+     151             :   ros::ServiceServer service_eland_;
+     152             : 
+     153             :   bool callbackTakeoff(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     154             :   bool callbackLand(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     155             :   bool callbackELand(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     156             : 
+     157             :   // | ------------------ dynamics constraints ------------------ |
+     158             : 
+     159             :   double _horizontal_speed_;
+     160             :   double _vertical_speed_;
+     161             :   double _takeoff_speed_;
+     162             :   double _landing_speed_;
+     163             :   double _elanding_speed_;
+     164             : 
+     165             :   double _horizontal_acceleration_;
+     166             :   double _vertical_acceleration_;
+     167             :   double _takeoff_acceleration_;
+     168             :   double _landing_acceleration_;
+     169             :   double _elanding_acceleration_;
+     170             : 
+     171             :   double _heading_rate_;
+     172             :   double _heading_gain_;
+     173             : 
+     174             :   double _max_position_difference_;
+     175             : 
+     176             :   // | -------------------------- goal -------------------------- |
+     177             : 
+     178             :   double            goal_x_, goal_y_, goal_z_, goal_heading_;
+     179             :   std::atomic<bool> have_goal_ = false;
+     180             :   std::mutex        mutex_goal_;
+     181             : 
+     182             :   // | ---------------- tracker's internal state ---------------- |
+     183             : 
+     184             :   double     state_x_, state_y_, state_z_, state_heading_;
+     185             :   double     speed_x_, speed_y_, speed_heading_;
+     186             :   double     current_heading_, current_vertical_direction_, current_vertical_speed_, current_horizontal_speed_;
+     187             :   double     current_horizontal_acceleration_, current_vertical_acceleration_;
+     188             :   std::mutex mutex_state_;
+     189             : 
+     190             :   // | -------------------- tracker's output -------------------- |
+     191             : 
+     192             :   mrs_msgs::TrackerCommand position_output_;
+     193             : 
+     194             :   // | ------------------------ profiler ------------------------ |
+     195             : 
+     196             :   mrs_lib::Profiler profiler_;
+     197             :   bool              _profiler_enabled_ = false;
+     198             : 
+     199             :   // | ----------------------- constraints ---------------------- |
+     200             : 
+     201             :   mrs_msgs::DynamicsConstraints constraints_;
+     202             :   std::mutex                    mutex_constraints_;
+     203             : };
+     204             : 
+     205             : //}
+     206             : 
+     207             : // | -------------- tracker's interface routines -------------- |
+     208             : 
+     209             : /* //{ initialize() */
+     210             : 
+     211          94 : bool LandoffTracker::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     212             :                                 std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     213             : 
+     214          94 :   this->common_handlers_  = common_handlers;
+     215          94 :   this->private_handlers_ = private_handlers;
+     216             : 
+     217          94 :   _uav_name_ = common_handlers->uav_name;
+     218             : 
+     219          94 :   nh_ = nh;
+     220             : 
+     221          94 :   ros::Time::waitForValid();
+     222             : 
+     223             :   // --------------------------------------------------------------
+     224             :   // |                     loading parameters                     |
+     225             :   // --------------------------------------------------------------
+     226             : 
+     227             :   // | ---------- loading params using the parent's nh ---------- |
+     228             : 
+     229         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     230             : 
+     231          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     232             : 
+     233          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     234           0 :     ROS_ERROR("[LandoffTracker]: Could not load all parameters!");
+     235           0 :     return false;
+     236             :   }
+     237             : 
+     238             :   // | --------------- loading plugin's parameters -------------- |
+     239             : 
+     240          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/landoff_tracker.yaml");
+     241          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/landoff_tracker.yaml");
+     242             : 
+     243         188 :   const std::string yaml_prefix = "mrs_uav_trackers/landoff_tracker/";
+     244             : 
+     245          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_speed", _horizontal_speed_);
+     246          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_acceleration", _horizontal_acceleration_);
+     247             : 
+     248          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     249          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_acceleration", _vertical_acceleration_);
+     250             : 
+     251          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_speed", _takeoff_speed_);
+     252          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_acceleration", _takeoff_acceleration_);
+     253             : 
+     254          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_speed", _landing_speed_);
+     255          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_acceleration", _landing_acceleration_);
+     256             : 
+     257          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_speed", _elanding_speed_);
+     258          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_acceleration", _elanding_acceleration_);
+     259             : 
+     260          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     261          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_gain", _heading_gain_);
+     262             : 
+     263          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "main_timer_rate", _main_timer_rate_);
+     264             : 
+     265          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "landing_reference", _landing_reference_);
+     266             : 
+     267          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_position_difference", _max_position_difference_);
+     268             : 
+     269          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains", _takeoff_disable_lateral_gains_);
+     270          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains_z", _takeoff_disable_lateral_gains_z_);
+     271             : 
+     272          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     273           0 :     ROS_ERROR("[LandoffTracker]: Could not load all parameters!");
+     274           0 :     return false;
+     275             :   }
+     276             : 
+     277          94 :   _tracker_dt_ = 1.0 / double(_main_timer_rate_);
+     278             : 
+     279          94 :   ROS_INFO("[LandoffTracker]: tracker_dt: %f", _tracker_dt_);
+     280             : 
+     281          94 :   state_x_       = 0;
+     282          94 :   state_y_       = 0;
+     283          94 :   state_z_       = 0;
+     284          94 :   state_heading_ = 0;
+     285             : 
+     286          94 :   speed_x_       = 0;
+     287          94 :   speed_y_       = 0;
+     288          94 :   speed_heading_ = 0;
+     289             : 
+     290          94 :   current_horizontal_speed_ = 0;
+     291          94 :   current_vertical_speed_   = 0;
+     292             : 
+     293          94 :   current_horizontal_acceleration_ = 0;
+     294          94 :   current_vertical_acceleration_   = 0;
+     295             : 
+     296          94 :   current_vertical_direction_ = 0;
+     297             : 
+     298          94 :   current_state_vertical_  = LANDED_STATE;
+     299          94 :   previous_state_vertical_ = LANDED_STATE;
+     300             : 
+     301          94 :   current_state_horizontal_  = LANDED_STATE;
+     302          94 :   previous_state_horizontal_ = LANDED_STATE;
+     303             : 
+     304             :   // | ------------------------ profiler ------------------------ |
+     305             : 
+     306          94 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "LandoffTracker", _profiler_enabled_);
+     307             : 
+     308             :   // | ------------------------ services ------------------------ |
+     309             : 
+     310          94 :   service_takeoff_ = nh_.advertiseService("takeoff", &LandoffTracker::callbackTakeoff, this);
+     311          94 :   service_land_    = nh_.advertiseService("land", &LandoffTracker::callbackLand, this);
+     312          94 :   service_eland_   = nh_.advertiseService("eland", &LandoffTracker::callbackELand, this);
+     313             : 
+     314             :   // | ------------------------- timers ------------------------- |
+     315             : 
+     316          94 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &LandoffTracker::timerMain, this, false, false);
+     317             : 
+     318             :   // | ----------------------- finish init ---------------------- |
+     319             : 
+     320          94 :   is_initialized_ = true;
+     321             : 
+     322          94 :   ROS_INFO("[LandoffTracker]: initialized");
+     323             : 
+     324          94 :   return true;
+     325             : }
+     326             : 
+     327             : //}
+     328             : 
+     329             : /* //{ activate() */
+     330             : 
+     331          45 : std::tuple<bool, std::string> LandoffTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     332             : 
+     333          90 :   std::stringstream ss;
+     334             : 
+     335          45 :   if (!got_uav_state_) {
+     336             : 
+     337           0 :     ss << "odometry not set";
+     338           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+     339           0 :     return std::tuple(false, ss.str());
+     340             :   }
+     341             : 
+     342             :   // copy member variables
+     343          90 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     344             : 
+     345             :   double uav_heading;
+     346             : 
+     347             :   try {
+     348          45 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     349             :   }
+     350           0 :   catch (...) {
+     351             : 
+     352           0 :     ss << "could not initialize the UAV heading";
+     353           0 :     ROS_ERROR_STREAM("[LandoffTracker]: " << ss.str());
+     354           0 :     return std::tuple(false, ss.str());
+     355             :   }
+     356             : 
+     357             :   // --------------------------------------------------------------
+     358             :   // |                      initial condition                     |
+     359             :   // --------------------------------------------------------------
+     360             : 
+     361             :   {
+     362          90 :     std::scoped_lock lock(mutex_goal_);
+     363             : 
+     364             :     // the last command is usable
+     365          45 :     state_x_       = uav_state.pose.position.x;
+     366          45 :     state_y_       = uav_state.pose.position.y;
+     367          45 :     state_z_       = uav_state.pose.position.z;
+     368          45 :     state_heading_ = uav_heading;
+     369             : 
+     370          45 :     speed_x_         = uav_state.velocity.linear.x;
+     371          45 :     speed_y_         = uav_state.velocity.linear.y;
+     372          45 :     current_heading_ = atan2(speed_y_, speed_x_);
+     373             : 
+     374          45 :     current_horizontal_speed_ = sqrt(pow(speed_x_, 2) + pow(speed_y_, 2));
+     375             : 
+     376          45 :     current_vertical_speed_     = fabs(uav_state.velocity.linear.z);
+     377          45 :     current_vertical_direction_ = uav_state.velocity.linear.z > 0 ? +1 : -1;
+     378             : 
+     379          45 :     current_horizontal_acceleration_ = 0;
+     380          45 :     current_vertical_acceleration_   = 0;
+     381             : 
+     382          45 :     goal_heading_ = uav_heading;
+     383             : 
+     384          45 :     ROS_INFO("[LandoffTracker]: initial condition: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", state_x_, state_y_, state_z_, state_heading_);
+     385             :   }
+     386             : 
+     387             :   // --------------------------------------------------------------
+     388             :   // |          horizontal initial conditions prediction          |
+     389             :   // --------------------------------------------------------------
+     390             : 
+     391             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     392             : 
+     393             :   {
+     394          45 :     std::scoped_lock lock(mutex_state_);
+     395             : 
+     396          45 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     397          45 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     398          45 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     399          45 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     400             :   }
+     401             : 
+     402             :   // --------------------------------------------------------------
+     403             :   // |           vertical initial conditions prediction           |
+     404             :   // --------------------------------------------------------------
+     405             : 
+     406             :   double vertical_t_stop, vertical_stop_dist;
+     407             : 
+     408             :   {
+     409          45 :     std::scoped_lock lock(mutex_state_);
+     410             : 
+     411          45 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     412          45 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     413             :   }
+     414             : 
+     415             :   // --------------------------------------------------------------
+     416             :   // |               heading initial condition prediction             |
+     417             :   // --------------------------------------------------------------
+     418             : 
+     419             :   {
+     420          45 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     421             : 
+     422          45 :     goal_x_ = state_x_ + stop_dist_x;
+     423          45 :     goal_y_ = state_y_ + stop_dist_y;
+     424          45 :     goal_z_ = state_z_ + vertical_stop_dist;
+     425             :   }
+     426             : 
+     427          45 :   landing_        = false;
+     428          45 :   taking_off_     = false;
+     429          45 :   is_active_      = true;
+     430          45 :   have_goal_      = false;
+     431          45 :   cause_failsafe_ = false;
+     432             : 
+     433          45 :   timer_main_.start();
+     434             : 
+     435             :   {
+     436          90 :     std::scoped_lock lock(mutex_goal_);
+     437             : 
+     438          45 :     ROS_INFO("[LandoffTracker]: stopping goal: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     439             :   }
+     440             : 
+     441          45 :   changeState(STOP_MOTION_STATE);
+     442             : 
+     443          45 :   ss << "activated";
+     444          45 :   ROS_INFO_STREAM("[LandoffTracker]: " << ss.str());
+     445             : 
+     446          45 :   return std::tuple(true, ss.str());
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : /* //{ deactivate() */
+     452             : 
+     453          61 : void LandoffTracker::deactivate(void) {
+     454             : 
+     455          61 :   is_active_                = false;
+     456          61 :   landing_                  = false;
+     457          61 :   taking_off_               = false;
+     458          61 :   current_state_vertical_   = IDLE_STATE;
+     459          61 :   current_state_horizontal_ = IDLE_STATE;
+     460             : 
+     461          61 :   timer_main_.stop();
+     462             : 
+     463          61 :   ROS_INFO("[LandoffTracker]: deactivated");
+     464          61 : }
+     465             : 
+     466             : //}
+     467             : 
+     468             : /* //{ resetStatic() */
+     469             : 
+     470           0 : bool LandoffTracker::resetStatic(void) {
+     471             : 
+     472           0 :   return false;
+     473             : }
+     474             : 
+     475             : //}
+     476             : 
+     477             : /* //{ update() */
+     478             : 
+     479      127134 : std::optional<mrs_msgs::TrackerCommand> LandoffTracker::update(const mrs_msgs::UavState&                                           uav_state,
+     480             :                                                                [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput& last_control_output) {
+     481             : 
+     482      381402 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     483      381402 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     484             : 
+     485             :   {
+     486      127134 :     std::scoped_lock lock(mutex_uav_state_);
+     487             : 
+     488      127134 :     uav_state_ = uav_state;
+     489             : 
+     490      127134 :     got_uav_state_ = true;
+     491             :   }
+     492             : 
+     493             :   // up to this part the update() method is evaluated even when the tracker is not active
+     494      127134 :   if (!is_active_ || cause_failsafe_) {
+     495      109645 :     return {};
+     496             :   }
+     497             : 
+     498       17489 :   position_output_.header.stamp    = ros::Time::now();
+     499       17489 :   position_output_.header.frame_id = uav_state_.header.frame_id;
+     500             : 
+     501             :   {
+     502       17489 :     std::scoped_lock lock(mutex_state_);
+     503             : 
+     504       17489 :     position_output_.position.x = state_x_;
+     505       17489 :     position_output_.position.y = state_y_;
+     506       17489 :     position_output_.position.z = state_z_;
+     507       17489 :     position_output_.heading    = state_heading_;
+     508             : 
+     509       17489 :     position_output_.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     510       17489 :     position_output_.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     511       17489 :     position_output_.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     512       17489 :     position_output_.heading_rate = speed_heading_;
+     513             : 
+     514       17489 :     position_output_.use_position_vertical   = 1;
+     515       17489 :     position_output_.use_position_horizontal = 1;
+     516       17489 :     position_output_.use_heading             = 1;
+     517       17489 :     position_output_.use_heading_rate        = 1;
+     518       17489 :     position_output_.use_velocity_vertical   = 1;
+     519       17489 :     position_output_.use_velocity_horizontal = 1;
+     520             :   }
+     521             : 
+     522             :   {
+     523       34978 :     std::scoped_lock lock(mutex_last_control_output_);
+     524             : 
+     525       17489 :     last_control_output_ = last_control_output;
+     526             :   }
+     527             : 
+     528       17489 :   if (_takeoff_disable_lateral_gains_ && taking_off_ && uav_state_.pose.position.z < _takeoff_disable_lateral_gains_z_) {
+     529           0 :     position_output_.disable_position_gains = true;
+     530             :   } else {
+     531       17489 :     position_output_.disable_position_gains = false;
+     532             :   }
+     533             : 
+     534       17489 :   if (taking_off_) {
+     535        8707 :     position_output_.disable_antiwindups = true;
+     536             :   } else {
+     537        8782 :     position_output_.disable_antiwindups = false;
+     538             :   }
+     539             : 
+     540       17489 :   return {position_output_};
+     541             : }
+     542             : 
+     543             : //}
+     544             : 
+     545             : /* //{ getStatus() */
+     546             : 
+     547        2213 : const mrs_msgs::TrackerStatus LandoffTracker::getStatus() {
+     548             : 
+     549        2213 :   mrs_msgs::TrackerStatus tracker_status;
+     550             : 
+     551        2213 :   tracker_status.active            = is_active_;
+     552        2213 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     553             : 
+     554        2213 :   bool hovering = current_state_vertical_ == HOVER_STATE && current_state_horizontal_ == HOVER_STATE;
+     555        2213 :   bool idling   = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     556             : 
+     557        2213 :   tracker_status.have_goal = landing_ || taking_off_ || !(hovering || idling);
+     558             : 
+     559        2213 :   tracker_status.tracking_trajectory = false;
+     560             : 
+     561        2213 :   return tracker_status;
+     562             : }
+     563             : 
+     564             : //}
+     565             : 
+     566             : /* //{ enableCallbacks() */
+     567             : 
+     568         297 : const std_srvs::SetBoolResponse::ConstPtr LandoffTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+     569             : 
+     570         594 :   std_srvs::SetBoolResponse res;
+     571         594 :   std::stringstream         ss;
+     572             : 
+     573         297 :   if (cmd->data != callbacks_enabled_) {
+     574             : 
+     575          20 :     callbacks_enabled_ = cmd->data;
+     576             : 
+     577          20 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     578          20 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     579             : 
+     580             :   } else {
+     581             : 
+     582         277 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     583         277 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     584             :   }
+     585             : 
+     586         297 :   res.message = ss.str();
+     587         297 :   res.success = true;
+     588             : 
+     589         594 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     590             : }
+     591             : 
+     592             : //}
+     593             : 
+     594             : /* switchOdometrySource() //{ */
+     595             : 
+     596           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState& new_uav_state) {
+     597             : 
+     598           0 :   std::scoped_lock lock(mutex_goal_, mutex_state_);
+     599             : 
+     600           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     601             : 
+     602           0 :   double old_heading  = 0;
+     603           0 :   double new_heading  = 0;
+     604           0 :   bool   got_headings = true;
+     605             : 
+     606             :   try {
+     607           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     608             :   }
+     609           0 :   catch (...) {
+     610           0 :     ROS_ERROR_THROTTLE(1.0, "[LandoffTracker]: could not calculate the old UAV heading");
+     611           0 :     got_headings = false;
+     612             :   }
+     613             : 
+     614             :   try {
+     615           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+     616             :   }
+     617           0 :   catch (...) {
+     618           0 :     ROS_ERROR_THROTTLE(1.0, "[LandoffTracker]: could not calculate the new UAV heading");
+     619           0 :     got_headings = false;
+     620             :   }
+     621             : 
+     622           0 :   std_srvs::TriggerResponse res;
+     623             : 
+     624           0 :   if (!got_headings) {
+     625           0 :     res.message = "could not calculate the heading difference";
+     626           0 :     res.success = false;
+     627             : 
+     628           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     629             :   }
+     630             : 
+     631             :   // | --------- recalculate the goal to new coordinates -------- |
+     632             : 
+     633           0 :   double dx       = new_uav_state.pose.position.x - uav_state.pose.position.x;
+     634           0 :   double dy       = new_uav_state.pose.position.y - uav_state.pose.position.y;
+     635           0 :   double dz       = new_uav_state.pose.position.z - uav_state.pose.position.z;
+     636           0 :   double dheading = new_heading - old_heading;
+     637             : 
+     638           0 :   goal_x_ += dx;
+     639           0 :   goal_y_ += dy;
+     640           0 :   goal_z_ += dz;
+     641           0 :   goal_heading_ += dheading;
+     642             : 
+     643             :   // | -------------------- update the state -------------------- |
+     644             : 
+     645           0 :   state_x_ += dx;
+     646           0 :   state_y_ += dy;
+     647           0 :   state_z_ += dz;
+     648           0 :   state_heading_ += dheading;
+     649             : 
+     650           0 :   current_heading_ = atan2(goal_y_ - state_y_, goal_x_ - state_x_);
+     651             : 
+     652           0 :   res.message = "odometry source switched";
+     653           0 :   res.success = true;
+     654             : 
+     655           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     656             : }
+     657             : 
+     658             : //}
+     659             : 
+     660             : /* //{ hover() */
+     661           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     662             : 
+     663           0 :   std::scoped_lock lock(mutex_main_timer_);
+     664             : 
+     665             :   // copy member variables
+     666           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     667             : 
+     668           0 :   auto [current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] =
+     669           0 :       mrs_lib::get_mutexed(mutex_state_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+     670             : 
+     671           0 :   std_srvs::TriggerResponse res;
+     672             : 
+     673             :   // --------------------------------------------------------------
+     674             :   // |          horizontal initial conditions prediction          |
+     675             :   // --------------------------------------------------------------
+     676             :   {
+     677           0 :     std::scoped_lock lock(mutex_state_);
+     678             : 
+     679           0 :     current_horizontal_speed_ = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2));
+     680           0 :     current_vertical_speed_   = uav_state.velocity.linear.z;
+     681           0 :     current_heading_          = atan2(uav_state.velocity.linear.y, uav_state.velocity.linear.x);
+     682             :   }
+     683             : 
+     684             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     685             : 
+     686           0 :   horizontal_t_stop    = current_horizontal_speed / _horizontal_acceleration_;
+     687           0 :   horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed) / 2.0;
+     688           0 :   stop_dist_x          = cos(current_heading) * horizontal_stop_dist;
+     689           0 :   stop_dist_y          = sin(current_heading) * horizontal_stop_dist;
+     690             : 
+     691             :   // --------------------------------------------------------------
+     692             :   // |           vertical initial conditions prediction           |
+     693             :   // --------------------------------------------------------------
+     694             : 
+     695           0 :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+     696           0 :   double vertical_stop_dist = current_vertical_direction * (vertical_t_stop * current_vertical_speed) / 2.0;
+     697             : 
+     698             :   // --------------------------------------------------------------
+     699             :   // |                        set the goal                        |
+     700             :   // --------------------------------------------------------------
+     701             : 
+     702             :   {
+     703           0 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+     704             : 
+     705           0 :     goal_x_ = state_x_ + stop_dist_x;
+     706           0 :     goal_y_ = state_y_ + stop_dist_y;
+     707           0 :     goal_z_ = state_z_ + vertical_stop_dist;
+     708             :   }
+     709             : 
+     710           0 :   res.message = "hover initiated";
+     711           0 :   res.success = true;
+     712             : 
+     713           0 :   changeState(STOP_MOTION_STATE);
+     714             : 
+     715           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     716             : }
+     717             : 
+     718             : //}
+     719             : 
+     720             : /* //{ startTrajectoryTracking() */
+     721             : 
+     722           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     723           0 :   return std_srvs::TriggerResponse::Ptr();
+     724             : }
+     725             : 
+     726             : //}
+     727             : 
+     728             : /* //{ stopTrajectoryTracking() */
+     729             : 
+     730           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     731           0 :   return std_srvs::TriggerResponse::Ptr();
+     732             : }
+     733             : 
+     734             : //}
+     735             : 
+     736             : /* //{ resumeTrajectoryTracking() */
+     737             : 
+     738           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     739           0 :   return std_srvs::TriggerResponse::Ptr();
+     740             : }
+     741             : 
+     742             : //}
+     743             : 
+     744             : /* //{ gotoTrajectoryStart() */
+     745             : 
+     746           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     747           0 :   return std_srvs::TriggerResponse::Ptr();
+     748             : }
+     749             : 
+     750             : //}
+     751             : 
+     752             : /* //{ setConstraints() */
+     753             : 
+     754         325 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LandoffTracker::setConstraints([
+     755             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd) {
+     756             : 
+     757             : 
+     758         325 :   mrs_lib::set_mutexed(mutex_constraints_, cmd->constraints, constraints_);
+     759             : 
+     760         325 :   ROS_INFO("[LandoffTracker]: updating constraints");
+     761             : 
+     762         650 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     763         325 :   res.success = true;
+     764         325 :   res.message = "constraints updated";
+     765             : 
+     766         650 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     767             : }
+     768             : 
+     769             : //}
+     770             : 
+     771             : /* //{ setReference() */
+     772             : 
+     773           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr LandoffTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+     774             : 
+     775           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     776             : }
+     777             : 
+     778             : //}
+     779             : 
+     780             : /* //{ setVelocityReference() */
+     781             : 
+     782           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr LandoffTracker::setVelocityReference([
+     783             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+     784           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     785             : }
+     786             : 
+     787             : //}
+     788             : 
+     789             : /* //{ setTrajectoryReference() */
+     790             : 
+     791           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LandoffTracker::setTrajectoryReference([
+     792             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+     793           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     794             : }
+     795             : 
+     796             : //}
+     797             : 
+     798             : // | ----------------- state machine routines ----------------- |
+     799             : 
+     800             : /* //{ changeStateHorizontal() */
+     801             : 
+     802         179 : void LandoffTracker::changeStateHorizontal(States_t new_state) {
+     803             : 
+     804         179 :   previous_state_horizontal_ = current_state_horizontal_;
+     805         179 :   current_state_horizontal_  = new_state;
+     806             : 
+     807         179 :   switch (current_state_horizontal_) {
+     808             : 
+     809          57 :     case STOPPING_STATE: {
+     810             : 
+     811         114 :       std::scoped_lock lock(mutex_state_);
+     812          57 :       current_horizontal_speed_ = 0;
+     813             : 
+     814          57 :       break;
+     815             :     };
+     816             : 
+     817         122 :     default: {
+     818             : 
+     819         122 :       break;
+     820             :     }
+     821             :   }
+     822             : 
+     823         179 :   ROS_INFO("[LandoffTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+     824         179 : }
+     825             : 
+     826             : //}
+     827             : 
+     828             : /* //{ changeStateVertical() */
+     829             : 
+     830         219 : void LandoffTracker::changeStateVertical(States_t new_state) {
+     831             : 
+     832         219 :   previous_state_vertical_ = current_state_vertical_;
+     833         219 :   current_state_vertical_  = new_state;
+     834             : 
+     835         219 :   switch (current_state_vertical_) {
+     836             : 
+     837          44 :     case HOVER_STATE: {
+     838          44 :       taking_off_ = false;
+     839          44 :       break;
+     840             :     }
+     841             : 
+     842         175 :     default: {
+     843         175 :       break;
+     844             :     }
+     845             :   }
+     846             : 
+     847         219 :   ROS_INFO("[LandoffTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+     848         219 : }
+     849             : 
+     850             : //}
+     851             : 
+     852             : /* //{ changeState() */
+     853             : 
+     854         146 : void LandoffTracker::changeState(States_t new_state) {
+     855             : 
+     856         146 :   changeStateVertical(new_state);
+     857         146 :   changeStateHorizontal(new_state);
+     858         146 : }
+     859             : 
+     860             : //}
+     861             : 
+     862             : // | --------------------- motion routines -------------------- |
+     863             : 
+     864             : /* //{ stopHorizontalMotion() */
+     865             : 
+     866         634 : void LandoffTracker::stopHorizontalMotion(void) {
+     867             : 
+     868             :   {
+     869        1268 :     std::scoped_lock lock(mutex_state_);
+     870             : 
+     871         634 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     872             : 
+     873         634 :     if (current_horizontal_speed_ < 0) {
+     874         350 :       current_horizontal_speed_        = 0;
+     875         350 :       current_horizontal_acceleration_ = 0;
+     876             :     } else {
+     877         284 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     878             :     }
+     879             :   }
+     880         634 : }
+     881             : 
+     882             : //}
+     883             : 
+     884             : /* //{ stopVerticalMotion() */
+     885             : 
+     886         634 : void LandoffTracker::stopVerticalMotion(void) {
+     887             : 
+     888             :   {
+     889        1268 :     std::scoped_lock lock(mutex_state_);
+     890             : 
+     891         634 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     892             : 
+     893         634 :     if (current_vertical_speed_ < 0) {
+     894          46 :       current_vertical_speed_        = 0;
+     895          46 :       current_vertical_acceleration_ = 0;
+     896             :     } else {
+     897         588 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     898             :     }
+     899             :   }
+     900         634 : }
+     901             : 
+     902             : //}
+     903             : 
+     904             : /* //{ accelerateVertical() */
+     905             : 
+     906       15863 : void LandoffTracker::accelerateVertical(void) {
+     907             : 
+     908             :   // copy member variables
+     909       15863 :   auto [current_vertical_speed, state_z] = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_, state_z_);
+     910       15863 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     911       15863 :   auto constraints                       = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     912             : 
+     913             :   double used_acceleration;
+     914             :   double used_speed;
+     915             : 
+     916       15863 :   if (taking_off_) {
+     917             : 
+     918        4480 :     used_speed        = _takeoff_speed_;
+     919        4480 :     used_acceleration = _takeoff_acceleration_;
+     920             : 
+     921        4480 :     if (used_speed > constraints.vertical_ascending_speed) {
+     922           0 :       used_speed = constraints.vertical_ascending_speed;
+     923           0 :       ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating takeoff speed");
+     924             :     }
+     925             : 
+     926        4480 :     if (used_acceleration > constraints.vertical_ascending_acceleration) {
+     927           0 :       used_acceleration = constraints.vertical_ascending_acceleration;
+     928           0 :       ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating takeoff acceleration");
+     929             :     }
+     930             : 
+     931       11383 :   } else if (landing_) {
+     932             : 
+     933       11383 :     if (elanding_) {
+     934             : 
+     935        7161 :       used_speed        = _elanding_speed_;
+     936        7161 :       used_acceleration = _elanding_acceleration_;
+     937             : 
+     938             :     } else {
+     939             : 
+     940        4222 :       used_speed        = _landing_speed_;
+     941        4222 :       used_acceleration = _landing_acceleration_;
+     942             : 
+     943        4222 :       if (used_speed > constraints.vertical_descending_speed) {
+     944           0 :         used_speed = constraints.vertical_descending_speed;
+     945           0 :         ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating landing speed");
+     946             :       }
+     947             : 
+     948        4222 :       if (used_acceleration > constraints.vertical_descending_acceleration) {
+     949           0 :         used_acceleration = constraints.vertical_descending_acceleration;
+     950           0 :         ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating landing acceleration");
+     951             :       }
+     952             :     }
+     953             : 
+     954             :   } else {
+     955             : 
+     956             :     // TODO take this from constraints
+     957           0 :     used_speed        = _vertical_speed_;
+     958           0 :     used_acceleration = _vertical_acceleration_;
+     959             :   }
+     960             : 
+     961             :   // set the right heading
+     962       15863 :   double tar_z = goal_z - state_z;
+     963             : 
+     964             :   // set the right vertical direction
+     965             :   {
+     966       15863 :     std::scoped_lock lock(mutex_state_);
+     967             : 
+     968       15863 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+     969             :   }
+     970             : 
+     971       15863 :   auto current_vertical_direction = mrs_lib::get_mutexed(mutex_state_, current_vertical_direction_);
+     972             : 
+     973             :   // calculate the time to stop and the distance it will take to stop [vertical]
+     974       15863 :   double vertical_t_stop    = current_vertical_speed / used_acceleration;
+     975       15863 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+     976       15863 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+     977             : 
+     978             :   {
+     979       31726 :     std::scoped_lock lock(mutex_state_);
+     980             : 
+     981       15863 :     current_vertical_speed_ += used_acceleration * _tracker_dt_;
+     982             : 
+     983       15863 :     if (current_vertical_speed_ >= used_speed) {
+     984        3925 :       current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     985        3925 :       current_vertical_acceleration_ = 0;
+     986             :     } else {
+     987       11938 :       current_vertical_acceleration_ = used_acceleration;
+     988             :     }
+     989             :   }
+     990             : 
+     991             :   // stopping condition to change to decelerate state
+     992             :   //
+     993             :   // It does not apply if landing or elanding, cause,
+     994             :   // it could potentially stop in mid air if odometry jumps (this happened),
+     995             :   // Instead, landing and elanding is stopped by sensing the throttle.
+     996       15863 :   if (!elanding_ && !landing_) {
+     997        4480 :     if (fabs(state_z + stop_dist_z - goal_z) < (2 * (used_speed * _tracker_dt_))) {
+     998             : 
+     999             :       {
+    1000          20 :         std::scoped_lock lock(mutex_state_);
+    1001             : 
+    1002          20 :         current_vertical_acceleration_ = 0;
+    1003             :       }
+    1004             : 
+    1005          20 :       changeStateVertical(DECELERATING_STATE);
+    1006             :     }
+    1007             :   }
+    1008       15863 : }
+    1009             : 
+    1010             : //}
+    1011             : 
+    1012             : /* //{ decelerateVertical() */
+    1013             : 
+    1014        4480 : void LandoffTracker::decelerateVertical(void) {
+    1015             : 
+    1016             :   double used_acceleration;
+    1017             : 
+    1018        4480 :   if (taking_off_) {
+    1019             : 
+    1020        4480 :     used_acceleration = _takeoff_acceleration_;
+    1021             : 
+    1022           0 :   } else if (landing_) {
+    1023             : 
+    1024           0 :     if (elanding_) {
+    1025             : 
+    1026           0 :       used_acceleration = _elanding_acceleration_;
+    1027             : 
+    1028             :     } else {
+    1029             : 
+    1030           0 :       used_acceleration = _landing_acceleration_;
+    1031             :     }
+    1032             : 
+    1033             :   } else {
+    1034           0 :     used_acceleration = _vertical_acceleration_;
+    1035             :   }
+    1036             : 
+    1037             :   {
+    1038        8960 :     std::scoped_lock lock(mutex_state_);
+    1039             : 
+    1040        4480 :     current_vertical_speed_ -= used_acceleration * _tracker_dt_;
+    1041             : 
+    1042        4480 :     if (current_vertical_speed_ < 0) {
+    1043          20 :       current_vertical_speed_ = 0;
+    1044             :     } else {
+    1045        4460 :       current_vertical_acceleration_ = -used_acceleration;
+    1046             :     }
+    1047             :   }
+    1048             : 
+    1049        4480 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1050             : 
+    1051        4480 :   if (current_vertical_speed == 0) {
+    1052             : 
+    1053             :     {
+    1054          20 :       std::scoped_lock lock(mutex_state_);
+    1055             : 
+    1056          20 :       current_vertical_acceleration_ = 0;
+    1057             :     }
+    1058             : 
+    1059          20 :     changeStateVertical(STOPPING_STATE);
+    1060             :   }
+    1061        4480 : }
+    1062             : 
+    1063             : //}
+    1064             : 
+    1065             : /* //{ stopHorizontal() */
+    1066             : 
+    1067       20343 : void LandoffTracker::stopHorizontal(void) {
+    1068             : 
+    1069             :   {
+    1070       20343 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1071             : 
+    1072       20343 :     double new_state_x = 0.95 * state_x_ + 0.05 * goal_x_;
+    1073       20343 :     double new_state_y = 0.95 * state_y_ + 0.05 * goal_y_;
+    1074             : 
+    1075       20343 :     double dist_x = new_state_x - state_x_;
+    1076       20343 :     double dist_y = new_state_y - state_y_;
+    1077             : 
+    1078       20343 :     double dt = 1.0 / _main_timer_rate_;
+    1079             : 
+    1080       20343 :     if (std::abs(dist_x / dt) > 1.0) {
+    1081           0 :       dist_x = mrs_lib::signum(dist_x) * (1.0 * dt);
+    1082             :     }
+    1083             : 
+    1084       20343 :     if (std::abs(dist_y / dt) > 1.0) {
+    1085           0 :       dist_y = mrs_lib::signum(dist_y) * (1.0 * dt);
+    1086             :     }
+    1087             : 
+    1088       20343 :     state_x_ += dist_x;
+    1089       20343 :     state_y_ += dist_y;
+    1090             : 
+    1091       20343 :     current_horizontal_acceleration_ = 0;
+    1092             :   }
+    1093       20343 : }
+    1094             : 
+    1095             : //}
+    1096             : 
+    1097             : /* //{ stopVertical() */
+    1098             : 
+    1099           0 : void LandoffTracker::stopVertical(void) {
+    1100             : 
+    1101             :   {
+    1102           0 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1103             : 
+    1104           0 :     double new_state_z = 0.95 * state_z_ + 0.05 * goal_z_;
+    1105             : 
+    1106           0 :     double dist_z = new_state_z - state_z_;
+    1107             : 
+    1108           0 :     double dt = 1.0 / _main_timer_rate_;
+    1109             : 
+    1110           0 :     if (std::abs(dist_z / dt) > 1.0) {
+    1111           0 :       dist_z = mrs_lib::signum(dist_z) * (1.0 * dt);
+    1112             :     }
+    1113             : 
+    1114           0 :     state_z_ += dist_z;
+    1115             : 
+    1116           0 :     current_vertical_acceleration_ = 0;
+    1117             :   }
+    1118           0 : }
+    1119             : 
+    1120             : //}
+    1121             : 
+    1122             : // | --------------------- timer routines --------------------- |
+    1123             : 
+    1124             : /* //{ timerMain() */
+    1125             : 
+    1126       23295 : void LandoffTracker::timerMain(const ros::TimerEvent& event) {
+    1127             : 
+    1128       23295 :   std::scoped_lock lock(mutex_main_timer_);
+    1129             : 
+    1130       23295 :   if (!is_active_) {
+    1131           0 :     return;
+    1132             :   }
+    1133             : 
+    1134             :   // copy member variables
+    1135       46590 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1136       23295 :   auto [state_x, state_y, state_z, current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] = mrs_lib::get_mutexed(
+    1137       23295 :       mutex_state_, state_x_, state_y_, state_z_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+    1138       23295 :   auto [goal_x, goal_y, goal_z] = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1139       46590 :   auto last_control_output      = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1140             : 
+    1141             :   double uav_x, uav_y, uav_z;
+    1142       23295 :   uav_x = uav_state.pose.position.x;
+    1143       23295 :   uav_y = uav_state.pose.position.y;
+    1144       23295 :   uav_z = uav_state.pose.position.z;
+    1145             : 
+    1146       69885 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _main_timer_rate_, 0.002, event);
+    1147       69885 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1148             : 
+    1149       23295 :   bool takeoff_saturated = false;
+    1150             : 
+    1151       23295 :   if (taking_off_) {
+    1152             : 
+    1153             :     // calculate the vector
+    1154        9512 :     double err_x      = uav_x - state_x;
+    1155        9512 :     double err_y      = uav_y - state_y;
+    1156        9512 :     double err_z      = uav_z - state_z;
+    1157        9512 :     double error_size = sqrt(pow(err_x, 2) + pow(err_y, 2) + pow(err_z, 2));
+    1158             : 
+    1159        9512 :     if (error_size > _max_position_difference_) {
+    1160             : 
+    1161             :       // calculate the potential next step
+    1162         104 :       double future_state_x = state_x + cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1163         104 :       double future_state_y = state_y + sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1164         104 :       double future_state_z = state_z + current_vertical_direction * current_vertical_speed * _tracker_dt_;
+    1165             : 
+    1166             :       // if the step would lead to a greater control error than the threshold
+    1167         104 :       if (mrs_lib::geometry::dist(vec3_t(future_state_x, future_state_y, future_state_z), vec3_t(uav_x, uav_y, uav_z)) > error_size) {
+    1168             : 
+    1169             :         // set this to true... later, we will not update the model if this is true, thus the tracker's motion will stop
+    1170             :         // => the tracker will wait for the controller
+    1171         104 :         takeoff_saturated = true;
+    1172             : 
+    1173         104 :         ROS_WARN_THROTTLE(
+    1174             :             0.1, "[LandoffTracker]: position difference %.3f > %.3f, saturating the motion. Reference: x=%.2f, y=%.2f, z=%.2f, Odometry: %.2f, %.2f, %.2f",
+    1175             :             error_size, _max_position_difference_, future_state_x, future_state_y, future_state_z, uav_x, uav_y, uav_z);
+    1176             :       }
+    1177             :     }
+    1178             : 
+    1179             :     // saturate while ramping up during takeoff
+    1180        9512 :     if (last_control_output.diagnostics.ramping_up) {
+    1181             : 
+    1182         448 :       ROS_INFO_THROTTLE(1.0, "[LandoffTracker]: waiting for the controller to rampup");
+    1183         448 :       takeoff_saturated = true;
+    1184             :     }
+    1185             :   }
+    1186             : 
+    1187       23295 :   if (!takeoff_saturated) {
+    1188             : 
+    1189       22743 :     switch (current_state_horizontal_) {
+    1190             : 
+    1191         634 :       case STOP_MOTION_STATE: {
+    1192             : 
+    1193         634 :         stopHorizontalMotion();
+    1194         634 :         break;
+    1195             :       }
+    1196             : 
+    1197       20343 :       case STOPPING_STATE: {
+    1198             : 
+    1199       20343 :         stopHorizontal();
+    1200       20343 :         break;
+    1201             :       }
+    1202             : 
+    1203        1766 :       default: {
+    1204             : 
+    1205        1766 :         break;
+    1206             :       }
+    1207             :     }
+    1208             : 
+    1209       22743 :     switch (current_state_vertical_) {
+    1210             : 
+    1211         634 :       case STOP_MOTION_STATE: {
+    1212             : 
+    1213         634 :         stopVerticalMotion();
+    1214         634 :         break;
+    1215             :       }
+    1216             : 
+    1217       15863 :       case ACCELERATING_STATE: {
+    1218             : 
+    1219       15863 :         accelerateVertical();
+    1220       15863 :         break;
+    1221             :       }
+    1222             : 
+    1223        4480 :       case DECELERATING_STATE: {
+    1224             : 
+    1225        4480 :         decelerateVertical();
+    1226        4480 :         break;
+    1227             :       }
+    1228             : 
+    1229           0 :       case STOPPING_STATE: {
+    1230             : 
+    1231           0 :         stopVertical();
+    1232           0 :         break;
+    1233             :       }
+    1234             : 
+    1235        1766 :       default: {
+    1236             : 
+    1237        1766 :         break;
+    1238             :       }
+    1239             :     }
+    1240             :   }
+    1241             : 
+    1242       23295 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1243         654 :     if (fabs(current_vertical_speed) <= 0.1 && fabs(current_horizontal_speed) <= 0.1) {
+    1244             : 
+    1245             :       // if the current motion was stopped (the conditions above) but we still have a goal (landing or taking off)
+    1246             :       // -> we should start accelerating towards the goal in the vertical direction
+    1247             :       // This is important, do not modify without testing, otherwise your landing routine may crash into the ground
+    1248             :       // while having large lateral speed
+    1249          57 :       if (have_goal_) {
+    1250             : 
+    1251          33 :         changeStateVertical(ACCELERATING_STATE);
+    1252          33 :         changeStateHorizontal(STOPPING_STATE);
+    1253             : 
+    1254             :       } else {
+    1255             : 
+    1256          24 :         changeState(STOPPING_STATE);
+    1257             : 
+    1258             :         {
+    1259          24 :           std::scoped_lock lock(mutex_state_);
+    1260             : 
+    1261          24 :           current_horizontal_speed_ = 0;
+    1262          24 :           current_vertical_speed_   = 0;
+    1263             :         }
+    1264             :       }
+    1265             :     }
+    1266             :   }
+    1267             : 
+    1268       23295 :   if (current_state_vertical_ == STOPPING_STATE && current_state_horizontal_ == STOPPING_STATE) {
+    1269             : 
+    1270          44 :     if (fabs(state_x - goal_x) > 1.0 || fabs(state_y - goal_y) > 1.0 || fabs(state_z - goal_z) > 1.0) {
+    1271             : 
+    1272           0 :       ROS_ERROR("[LandoffTracker]: distance to the goal is too large when STOPPING, this could have been caused by a race condition!");
+    1273           0 :       ROS_ERROR("[LandoffTracker]: call for Tomas!!");
+    1274             : 
+    1275           0 :       cause_failsafe_ = true;
+    1276             : 
+    1277           0 :       changeState(HOVER_STATE);
+    1278             : 
+    1279          44 :     } else if (fabs(state_x - goal_x) < 0.1 && fabs(state_y - goal_y) < 0.1 && fabs(state_z - goal_z) < 0.1) {
+    1280             : 
+    1281             :       {
+    1282          44 :         std::scoped_lock lock(mutex_state_);
+    1283             : 
+    1284          44 :         if (!taking_off_) {
+    1285          24 :           state_x_ = goal_x;
+    1286          24 :           state_y_ = goal_y;
+    1287          24 :           state_z_ = goal_z;
+    1288             :         }
+    1289             : 
+    1290          44 :         current_horizontal_speed_ = 0;
+    1291          44 :         current_vertical_speed_   = 0;
+    1292             :       }
+    1293             : 
+    1294          44 :       changeState(HOVER_STATE);
+    1295             : 
+    1296          44 :       have_goal_ = false;
+    1297             :     }
+    1298             :   }
+    1299             : 
+    1300       23295 :   if (current_state_horizontal_ == LANDED_STATE && current_state_vertical_ == LANDED_STATE) {
+    1301             :     {
+    1302           0 :       std::scoped_lock lock(mutex_state_);
+    1303             : 
+    1304           0 :       state_x_ = goal_x = uav_x;
+    1305           0 :       state_y_ = goal_y = uav_y;
+    1306           0 :       state_z_ = goal_z = uav_z;
+    1307             : 
+    1308           0 :       have_goal_ = false;
+    1309             :     }
+    1310             :   }
+    1311             : 
+    1312             :   // --------------------------------------------------------------
+    1313             :   // |              motion saturation during takeoff              |
+    1314             :   // --------------------------------------------------------------
+    1315             : 
+    1316             :   // update the inner states
+    1317       23295 :   if (!takeoff_saturated) {
+    1318             :     {
+    1319       22743 :       std::scoped_lock lock(mutex_state_);
+    1320             : 
+    1321       22743 :       state_x_ += cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1322       22743 :       state_y_ += sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1323       22743 :       state_z_ += current_vertical_direction * current_vertical_speed * _tracker_dt_;
+    1324             :     }
+    1325             :   }
+    1326             : 
+    1327             :   // --------------------------------------------------------------
+    1328             :   // |                        heading tracking                        |
+    1329             :   // --------------------------------------------------------------
+    1330             : 
+    1331             :   // compute the desired heading rate
+    1332             :   {
+    1333       46590 :     std::scoped_lock lock(mutex_state_);
+    1334             : 
+    1335             :     double current_heading_rate;
+    1336             : 
+    1337       23295 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1338           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1339             :     else
+    1340       23295 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1341             : 
+    1342       23295 :     if (current_heading_rate > _heading_rate_) {
+    1343           0 :       current_heading_rate = _heading_rate_;
+    1344       23295 :     } else if (current_heading_rate < -_heading_rate_) {
+    1345           0 :       current_heading_rate = -_heading_rate_;
+    1346             :     }
+    1347             : 
+    1348             :     // flap the resulted state_heading_ aroud PI
+    1349       23295 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1350             : 
+    1351       23295 :     if (state_heading_ > M_PI) {
+    1352           0 :       state_heading_ -= 2 * M_PI;
+    1353       23295 :     } else if (state_heading_ < -M_PI) {
+    1354           0 :       state_heading_ += 2 * M_PI;
+    1355             :     }
+    1356             : 
+    1357       23295 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1358       23295 :       state_heading_ = goal_heading_;
+    1359             :     }
+    1360             :   }
+    1361             : 
+    1362             :   // --------------------------------------------------------------
+    1363             :   // |                      landing setpoint                      |
+    1364             :   // --------------------------------------------------------------
+    1365             : 
+    1366       23295 :   if (landing_) {
+    1367             :     {
+    1368       11960 :       std::scoped_lock lock(mutex_goal_);
+    1369             : 
+    1370       11960 :       goal_z_ = uav_z + _landing_reference_;
+    1371             :     }
+    1372             :   }
+    1373             : }
+    1374             : 
+    1375             : //}
+    1376             : 
+    1377             : // | ------------------------ callbacks ----------------------- |
+    1378             : 
+    1379             : /* //{ callbackTakeoff() */
+    1380             : 
+    1381          20 : bool LandoffTracker::callbackTakeoff(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    1382             : 
+    1383          40 :   std::stringstream ss;
+    1384             : 
+    1385             :   // copy member variables
+    1386          40 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1387             : 
+    1388          20 :   double uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1389             : 
+    1390             :   double uav_x, uav_y, uav_z;
+    1391          20 :   uav_x = uav_state.pose.position.x;
+    1392          20 :   uav_y = uav_state.pose.position.y;
+    1393          20 :   uav_z = uav_state.pose.position.z;
+    1394             : 
+    1395          20 :   if (!is_active_) {
+    1396           0 :     ss << "can not takeoff, the tracker is not active";
+    1397           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1398           0 :     res.success = false;
+    1399           0 :     res.message = ss.str();
+    1400           0 :     return true;
+    1401             :   }
+    1402             : 
+    1403          20 :   if (!callbacks_enabled_) {
+    1404           0 :     ss << "can not takeoff, the callbacks are disabled";
+    1405           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1406           0 :     res.success = false;
+    1407           0 :     res.message = ss.str();
+    1408           0 :     return true;
+    1409             :   }
+    1410             : 
+    1411          20 :   if (req.goal < 0.5 || req.goal > 10.0) {
+    1412             : 
+    1413           0 :     ss << "can not takeoff, the goal should be within [0.5, 10.0] m!";
+    1414           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1415           0 :     res.success = false;
+    1416           0 :     res.message = ss.str();
+    1417           0 :     return true;
+    1418             :   }
+    1419             : 
+    1420             :   {
+    1421          40 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+    1422             : 
+    1423          20 :     state_x_ = uav_x;
+    1424          20 :     goal_x_  = uav_x;
+    1425             : 
+    1426          20 :     state_y_ = uav_y;
+    1427          20 :     goal_y_  = uav_y;
+    1428             : 
+    1429          20 :     state_z_ = uav_z;
+    1430          20 :     goal_z_  = uav_z + req.goal;
+    1431             : 
+    1432          20 :     state_heading_ = uav_heading;
+    1433          20 :     goal_heading_  = uav_heading;
+    1434             : 
+    1435          20 :     speed_x_                = 0;
+    1436          20 :     speed_y_                = 0;
+    1437          20 :     current_vertical_speed_ = 0;
+    1438             : 
+    1439          20 :     have_goal_ = true;
+    1440             :   }
+    1441             : 
+    1442          20 :   ROS_INFO("[LandoffTracker]: taking off");
+    1443             : 
+    1444          20 :   taking_off_ = true;
+    1445          20 :   landing_    = false;
+    1446          20 :   elanding_   = false;
+    1447             : 
+    1448          20 :   res.success = true;
+    1449          20 :   res.message = "taking off";
+    1450             : 
+    1451          20 :   changeState(STOP_MOTION_STATE);
+    1452             : 
+    1453          20 :   return true;
+    1454             : }
+    1455             : 
+    1456             : //}
+    1457             : 
+    1458             : /* //{ callbackLand() */
+    1459             : 
+    1460           5 : bool LandoffTracker::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1461             : 
+    1462          10 :   std::scoped_lock lock(mutex_main_timer_);
+    1463             : 
+    1464          10 :   std::stringstream ss;
+    1465             : 
+    1466             :   // copy member variables
+    1467          10 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1468             : 
+    1469           5 :   if (!is_active_) {
+    1470           0 :     ss << "can not land, the tracker is not active";
+    1471           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1472           0 :     res.success = false;
+    1473           0 :     res.message = ss.str();
+    1474           0 :     return true;
+    1475             :   }
+    1476             : 
+    1477             :   {
+    1478           5 :     std::scoped_lock lock(mutex_goal_);
+    1479             : 
+    1480           5 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1481             :   }
+    1482             : 
+    1483           5 :   ROS_INFO("[LandoffTracker]: landing");
+    1484             : 
+    1485           5 :   landing_    = true;
+    1486           5 :   elanding_   = false;
+    1487           5 :   taking_off_ = false;
+    1488           5 :   have_goal_  = true;
+    1489             : 
+    1490           5 :   res.success = true;
+    1491           5 :   res.message = "landing";
+    1492             : 
+    1493           5 :   changeState(STOP_MOTION_STATE);
+    1494             : 
+    1495           5 :   return true;
+    1496             : }
+    1497             : 
+    1498             : //}
+    1499             : 
+    1500             : /* //{ callbackELand() */
+    1501             : 
+    1502           8 : bool LandoffTracker::callbackELand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1503             : 
+    1504          16 :   std::scoped_lock lock(mutex_main_timer_);
+    1505             : 
+    1506          16 :   std::stringstream ss;
+    1507             : 
+    1508             :   // copy member variables
+    1509          16 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1510             : 
+    1511           8 :   if (!is_active_) {
+    1512             : 
+    1513           0 :     ss << "can not eland, the tracker is not active";
+    1514           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1515           0 :     res.success = false;
+    1516           0 :     res.message = ss.str();
+    1517           0 :     taking_off_ = false;
+    1518           0 :     landing_    = false;
+    1519           0 :     elanding_   = false;
+    1520           0 :     changeState(LANDED_STATE);
+    1521           0 :     return true;
+    1522             :   }
+    1523             : 
+    1524             :   {
+    1525           8 :     std::scoped_lock lock(mutex_goal_);
+    1526             : 
+    1527           8 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1528             :   }
+    1529             : 
+    1530           8 :   ROS_WARN("[LandoffTracker]: emergency landing");
+    1531             : 
+    1532           8 :   landing_    = true;
+    1533           8 :   elanding_   = true;
+    1534           8 :   taking_off_ = false;
+    1535           8 :   have_goal_  = true;
+    1536             : 
+    1537           8 :   res.success = true;
+    1538           8 :   res.message = "elanding";
+    1539             : 
+    1540           8 :   changeState(STOP_MOTION_STATE);
+    1541             : 
+    1542           8 :   return true;
+    1543             : }
+    1544             : 
+    1545             : //}
+    1546             : 
+    1547             : }  // namespace landoff_tracker
+    1548             : 
+    1549             : }  // namespace mrs_uav_trackers
+    1550             : 
+    1551             : #include <pluginlib/class_list_macros.h>
+    1552          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::landoff_tracker::LandoffTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..a184a445d4 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html @@ -0,0 +1,408 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..0549fe81374e6b7f724bd0b45f36c19b2bf94f41 GIT binary patch literal 4772 zcmV;V5?k$wP)1 zRCt{2U5jDsDh!P`uvf4*a8`&naQ9#3#(aekAn|GH-g4h-lk`L&A%qYDKbC1(mihhf z(W|`aDWXe>qz4wE;;L<#dr}lsEooyb`ys^)$F~r0&`SZrCA4Jij!*O)Q_+l zgNkS`nq#=`%Jf4sJ(Wi+8z`)Lo{BnYJEmXV1A4})Piee#uQ{kXOMO7qlh#OGcaIsw z(klZM(+rCJxqh(qkFBX}Q_D8U>~SIm_LJw_m)yfx3Og32sS`~@(hYB;@DqF|Y~x}L znkRbIFo!^{aC}=FxhZyV7Wjp1;WOhXO+od9bA_lnmI;a^u)(+x9L^8i&}o5P1DZzF zj`}U&9lR_87qRw`6fFhE&77#(f&(u(1LV2WtTxUu?;P={w=Ceq1zt!7a5imh?HSiN zLsmkNTR*5^p1asok_)Kf0Wkk{VRL+C9N;XNj=5E5is_UxaOU@(u6kN{J*TJ!bC0PF ze-{AqWCKcF1R473Gn|#x2l6`HcS~Un_c!a6&I!1q!&*GE+Bo^{upR^8yoT%f!RwcW zYhpaGRJETal%G-rz7csKyMz-|rU?t-X|&bvyU;jLW;x=Nse+lXk;d(f+lhfQ7r3y2 za{?$}28W~)1fv*Ws^UOzJ9HrMEIrE%pzan1aLxcL+n8-7n<6W;7XjH6djq-5?i<=v{@`GY{O^3a**z zvy?k80v0_fF|gmhOK9tvz}&=+Ca&JVKFbu|Xki0us_w)Mig4rMS2q;g3vciULj>wv zvJRhy2|j9= z;+9_43a<;1Y#Q2FjMlh6dX^ejD?j`C^m7xp5_Gj4QxQI z7;W&oTSmq6xjnIMyvBUbg`^V*)lbi6W2 zDAG;aF!1&I3d(W)cYK}if4aPM(ZKg#&sV-YUL}MDio0;}0j3#FAx&MFEdrpRueHT7 zKlhl}D(&lJ38)Kze^vigeY>i+{>*G6#M7VRfT~k7o05tL-eLMM;(0q~dH<}oXPi0T zFkKL~7Zd=;zC0&A+y z)s48$0l8K8kIt;7L&)Nnr~(^4sEtsBUZ%l23*1)(#OyhW7>K8zYOa@j-XNdrV%OBS zal-NI`kuL3nX`~>Q(W7aS#`ITfsCMs2C~bL(*-7~(2HWI`J}e(G6S&s?`1_=;eT>5vv*2uK@TtHOm$<>go<{eNWVt z`wbuWcVnT^$2G=T$9r@8R;yor1Ot$$dJNE8fYitkm}XmbGo!_n*zv(lN&l!MEZ3 zBDH4LXI|X4j+tyn?jgZ6D?n&*jgeAqu#aSa*Yq`BpM~!NrSN}3LucC@P^ku_0bj$a z8Nko0ncZaqJ56)gb3@f^ZXg574^T6tWx<<0v^{7>a&MK+{RE}(o7X-IY7Qj*qdci7 zAYIXgdc0@-lolYZXaoA%xjlPAwO!Sg0Ul6O%m-ZnkU`N%C<|m37{)08xfDO>*%Mxh zcTL=jA(jIBa2*_W)!bsoP}_i;`DT69zE3Dh4tS?X+|UfRF552lZF+EfBgFzbEi3}9 z((Xy+jW)#6KxP3HhiuyAg3@foWmi2=al2ho0|HVmFqH_0F=JB9P5ak_8RQZ~xrPb6 z=mAM-U4WcE`5Ly8^uWX1UD_=*(Vu!rGZ2&Hv;O9Srm6Ju(^%>jZ~Yl2a@v4 zFxyDSAF17x0oE)&#G*gh0u`qA3)PMxc9|ZpGvSP32Z`)!A|No5>+I%!LLaI5^KlLM z0BZ_seK8qOYzxz%d8+r^imON$M7(1lF#tOUJYD@{Yln2T)HPREFz7gfDIkz-BS z3IhzMKm**NwW(YUD@IVo`d3S4DmeygLrq!Wtp-=O@kJ$i5lghaHYql@H|#*y+w@{e zoE-+WnY8^?9zb@_t-Np9!J;W|CtS@&)2Y9Z8H!F)4hPj#MMdS<%C@K<16xeia zl~XjWdVA!(fUhIv_Xj|38`-IsX7zDW-4=EsHwjUYdP%Cy;c_ew{m%i9LL7NWqU4_( zd&n_ZDvnhCH45nea6-FX*)tcJH_ZnNV))JZyb9{}@w*Ekl~ySuIEN?KF@}@sruyTn&E_>eI$`*fEqFc`AiL-X5MhNs8)yO!2UNJ;3?so$T&- zIbXrBAzcN_-$A3Ucg{3li~))eX-@;Pf!w~nLn1kxXF}x(0={rN!g*mpC2 ziop*!oJLrR6Kaj)nk^nhQ49Jw2ueMGy4PdipmpKtoT$|NG02mGB}MoD_RU;r->u*~ zag-b>@!U|39m1}Ea_l3>KRNcoJnUG3;l)y?;-L2#y)3KpcT1^NrBIQ2(N5j+ODbZ=J+JVZ4zqgW(Clv$s1tE zdkfq1AJNhI1dvT}^%Qdnyb_AM(-#n^B$K+NQd%_&$Uv>4^ry8XLElDR)hnMFo~;=0 z*l)ruw0UY*#@c3W;myuz_F8kLY!5O$wYCS}u#p00OizpLNho`b?V*pkL5kKy6t7F@ zJ!ZH?VgPPCuF^hk?3fG>~`bjHVd8^%Qwrv%ByfqY#d{9M%*2!Y z{l-i@+S)lAHeVDA2?HUX0&?!bd?9$YslnWN-fKPnxw5jTi7|s!cE(wv!e+*;x zL9QFhpRpPIrvmbV$by~$hBS5K%?6qIN}v4@F!QA%?A@8!{dwL(7mEA32xL%J^81^&@dyMi?I{RE#f*Q?ie`a*#+)^OAcqkit7G>Juw1hgDa>S zKM5fyKV(snF)L#ZYU33((nx{2;UXm1fExz$?w5t3+EXmmPnS2NV=`9( z&hDb|h~oA4*(@}#H?slH_CI4`T^qshGtdbx0-ftoa5jPVY`|IB>_a@@t!}(>)y{p^ z5N~*iTrQU8UO<+faXb1!XgJNyUo7PZ$5OcA2W}Y_Re1Ib7O1=IjMJTU3whq7c|KQ} zo{S>T$rYSz>h9X7+DJT=tans_&C4g(3cuOq;s*C~kLc|Yvquzp%nZpa*aEI`F@F`` zMrOaBlD90>#MMWBsvDCTfqTtmhTAiQlR~$VyCl7t^U>{q2$39;a5Q;bH$92P{Hm;a z^$YB4y}bu7DuKePgVjdzf@~g8RP~z>uulmfpQ69gU6D=93fff&qP_1oXLM)7*xi($ z+{UL_xt3A}*+705GEMXEw2%Ex4MkfS6k|!7W-%SKerfZ$iMa zPXPH8!JQFj^2YN|T8fvw%c^h}LG9%)ij~GEL9-84F>$YN$k7Z=__XC{cD%YAyIgJ} zdp73Tr&H|36q)fJea{C+DemxGdkeIG2s2LIE5GpBi67t#?jCmgf3$+!L!fI->R6sQ z2sqPm`cOQmT}h?r4&*YkdO}XR)F@&-S85l@5%vsy@4TFu-oaG3shKsZcJ;C9$Ac!f zum=J`x9s-*boX2OtaB-=eA?Zijubdyq4@NK|hQK0qV=2q-e1YKx;3R$;Gh z(XZ{Sw1hqf*o!g0FBI3A7`Hj>_FHFilPIYZdwp~Uu1N!F?&_cVcz;4*_9BhXMuxYP zM5@K}L=H#$b1DL@?eQ#)HOleN0$CJC>|ZkuiPv$Re^|u&S*{ryFJi7)KrS=AhhaQs z+HV*bJ7&_7`ndLN1tG3~Tr=AEJ=Q51!3!mzw@+nx>Y=yJy z<{Tj0mMT9y}zI~KH@;@E3Ypu+BZ_!-b}r`{6*Lf6LJYpHmK+ zBq)sqewZhApZ351S>P?}Lef1HW?mJ8iWa!DfWNY8J#+P{+E0akqhgAX!_s-1Gsa5W7=r8$cp(rf~tYYyHG4? zjso7}nN7V9bS9T6DD}3Y3p<{^as%);EPN_Fd+jA1#v!HMnLh_9(S^L=jC4W@7kp%x z=36GD=F1H<38t}U%`;vi93ZD^W~qGKXveCv7O#ThFPyo0og7ZiTwx?evf~pO3c4Tv zX08H|OA$_Snb_6mr#L=EeXBDAcyfWR6o7xz@JFX%aw-`*4fCfs4)F8SaGzs?*wl5Q y_-m%&c?x#RI?zWHJ1k2<>cNtw8c%HWRQC@U&N?gtwp!f)0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
<unnamed>67.9 %322 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/line_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..03b9071b0f --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
<unnamed>67.9 %322 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/index-detail.html b/mrs_uav_trackers/src/line_tracker/index-detail.html new file mode 100644 index 0000000000..bdba2f21f3 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
<unnamed>67.9 %322 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/index-sort-f.html b/mrs_uav_trackers/src/line_tracker/index-sort-f.html new file mode 100644 index 0000000000..5a6a6c5a50 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/index-sort-l.html b/mrs_uav_trackers/src/line_tracker/index-sort-l.html new file mode 100644 index 0000000000..e480ed89f7 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/index.html b/mrs_uav_trackers/src/line_tracker/index.html new file mode 100644 index 0000000000..70b3a96689 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..722cff4ac3 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::line_tracker::LineTracker::deactivate()0
mrs_uav_trackers::line_tracker::LineTracker::resetStatic()0
mrs_uav_trackers::line_tracker::LineTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::line_tracker::LineTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_trackers::line_tracker::LineTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)1
mrs_uav_trackers::line_tracker::LineTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::line_tracker::LineTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)1
mrs_uav_trackers::line_tracker::LineTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)3
mrs_uav_trackers::line_tracker::LineTracker::changeState(mrs_uav_trackers::line_tracker::States_t)4
mrs_uav_trackers::line_tracker::LineTracker::changeStateVertical(mrs_uav_trackers::line_tracker::States_t)6
mrs_uav_trackers::line_tracker::LineTracker::changeStateHorizontal(mrs_uav_trackers::line_tracker::States_t)6
mrs_uav_trackers::line_tracker::LineTracker::stopVerticalMotion()13
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontalMotion()13
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontal()46
mrs_uav_trackers::line_tracker::LineTracker::decelerateVertical()100
mrs_uav_trackers::line_tracker::LineTracker::decelerateHorizontal()100
mrs_uav_trackers::line_tracker::LineTracker::getStatus()181
mrs_uav_trackers::line_tracker::LineTracker::accelerateVertical()199
mrs_uav_trackers::line_tracker::LineTracker::stopVertical()851
mrs_uav_trackers::line_tracker::LineTracker::accelerateHorizontal()1004
mrs_uav_trackers::line_tracker::LineTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)1738
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2234
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html new file mode 100644 index 0000000000..bd4d5529f7 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_trackers::line_tracker::LineTracker::deactivate()0
mrs_uav_trackers::line_tracker::LineTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)1
mrs_uav_trackers::line_tracker::LineTracker::changeState(mrs_uav_trackers::line_tracker::States_t)4
mrs_uav_trackers::line_tracker::LineTracker::resetStatic()0
mrs_uav_trackers::line_tracker::LineTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::line_tracker::LineTracker::stopVertical()851
mrs_uav_trackers::line_tracker::LineTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)3
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontal()46
mrs_uav_trackers::line_tracker::LineTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::accelerateVertical()199
mrs_uav_trackers::line_tracker::LineTracker::decelerateVertical()100
mrs_uav_trackers::line_tracker::LineTracker::stopVerticalMotion()13
mrs_uav_trackers::line_tracker::LineTracker::changeStateVertical(mrs_uav_trackers::line_tracker::States_t)6
mrs_uav_trackers::line_tracker::LineTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::accelerateHorizontal()1004
mrs_uav_trackers::line_tracker::LineTracker::decelerateHorizontal()100
mrs_uav_trackers::line_tracker::LineTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontalMotion()13
mrs_uav_trackers::line_tracker::LineTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::line_tracker::LineTracker::changeStateHorizontal(mrs_uav_trackers::line_tracker::States_t)6
mrs_uav_trackers::line_tracker::LineTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)1738
mrs_uav_trackers::line_tracker::LineTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)1
mrs_uav_trackers::line_tracker::LineTracker::getStatus()181
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2234
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.frameset.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.frameset.html new file mode 100644 index 0000000000..e94e1aecd5 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html new file mode 100644 index 0000000000..eeb288476a --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html @@ -0,0 +1,1359 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-25 05:42:57Functions:193063.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : 
+       8             : #include <mrs_lib/profiler.h>
+       9             : #include <mrs_lib/mutex.h>
+      10             : #include <mrs_lib/attitude_converter.h>
+      11             : #include <mrs_lib/utils.h>
+      12             : #include <mrs_lib/geometry/cyclic.h>
+      13             : #include <mrs_lib/geometry/misc.h>
+      14             : 
+      15             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      16             : 
+      17             : //}
+      18             : 
+      19             : /* defines //{ */
+      20             : 
+      21             : #define STOP_THR 1e-3
+      22             : 
+      23             : //}
+      24             : 
+      25             : /* using //{ */
+      26             : 
+      27             : using namespace Eigen;
+      28             : 
+      29             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      30             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      31             : 
+      32             : using radians  = mrs_lib::geometry::radians;
+      33             : using sradians = mrs_lib::geometry::sradians;
+      34             : 
+      35             : //}
+      36             : 
+      37             : namespace mrs_uav_trackers
+      38             : {
+      39             : 
+      40             : namespace line_tracker
+      41             : {
+      42             : 
+      43             : /* //{ class LineTracker */
+      44             : 
+      45             : // state machine
+      46             : typedef enum
+      47             : {
+      48             : 
+      49             :   IDLE_STATE,
+      50             :   STOP_MOTION_STATE,
+      51             :   ACCELERATING_STATE,
+      52             :   DECELERATING_STATE,
+      53             :   STOPPING_STATE,
+      54             : 
+      55             : } States_t;
+      56             : 
+      57             : const char *state_names[5] = {
+      58             : 
+      59             :     "IDLING", "STOPPING_MOTION", "ACCELERATING", "DECELERATING", "STOPPING"};
+      60             : 
+      61             : class LineTracker : public mrs_uav_managers::Tracker {
+      62             : public:
+      63             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      64             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      65             : 
+      66             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd);
+      67             :   void                          deactivate(void);
+      68             :   bool                          resetStatic(void);
+      69             : 
+      70             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState &uav_state, const mrs_uav_managers::Controller::ControlOutput &last_control_output);
+      71             :   const mrs_msgs::TrackerStatus             getStatus();
+      72             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd);
+      73             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      74             : 
+      75             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd);
+      76             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd);
+      77             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd);
+      78             : 
+      79             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      80             : 
+      81             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      82             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      83             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      84             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      85             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      86             : 
+      87             : private:
+      88             :   ros::NodeHandle nh_;
+      89             : 
+      90             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      91             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      92             : 
+      93             :   bool callbacks_enabled_ = true;
+      94             : 
+      95             :   std::string _uav_name_;
+      96             : 
+      97             :   void       mainTimer(const ros::TimerEvent &event);
+      98             :   ros::Timer main_timer_;
+      99             : 
+     100             :   // | ------------------------ uav state ----------------------- |
+     101             : 
+     102             :   mrs_msgs::UavState uav_state_;
+     103             :   bool               got_uav_state_ = false;
+     104             :   std::mutex         mutex_uav_state_;
+     105             : 
+     106             :   double uav_x_;
+     107             :   double uav_y_;
+     108             :   double uav_z_;
+     109             : 
+     110             :   // tracker's inner states
+     111             :   double _tracker_loop_rate_;
+     112             :   double _tracker_dt_;
+     113             :   bool   is_initialized_ = false;
+     114             :   bool   is_active_      = false;
+     115             : 
+     116             :   // | ----------------- internal state mmachine ---------------- |
+     117             : 
+     118             :   States_t current_state_vertical_    = IDLE_STATE;
+     119             :   States_t previous_state_vertical_   = IDLE_STATE;
+     120             :   States_t current_state_horizontal_  = IDLE_STATE;
+     121             :   States_t previous_state_horizontal_ = IDLE_STATE;
+     122             : 
+     123             :   void changeStateHorizontal(States_t new_state);
+     124             :   void changeStateVertical(States_t new_state);
+     125             :   void changeState(States_t new_state);
+     126             : 
+     127             :   void stopHorizontalMotion(void);
+     128             :   void stopVerticalMotion(void);
+     129             :   void accelerateHorizontal(void);
+     130             :   void accelerateVertical(void);
+     131             :   void decelerateHorizontal(void);
+     132             :   void decelerateVertical(void);
+     133             :   void stopHorizontal(void);
+     134             :   void stopVertical(void);
+     135             : 
+     136             :   // | ------------------ dynamics constraints ------------------ |
+     137             : 
+     138             :   double     _horizontal_speed_;
+     139             :   double     _vertical_speed_;
+     140             :   double     _horizontal_acceleration_;
+     141             :   double     _vertical_acceleration_;
+     142             :   double     _heading_rate_;
+     143             :   double     _heading_gain_;
+     144             :   std::mutex mutex_constraints_;
+     145             : 
+     146             :   // | ---------------------- desired goal ---------------------- |
+     147             : 
+     148             :   double     goal_x_;
+     149             :   double     goal_y_;
+     150             :   double     goal_z_;
+     151             :   double     goal_heading_;
+     152             :   bool       have_goal_ = false;
+     153             :   std::mutex mutex_goal_;
+     154             : 
+     155             :   // | ------------------- the state variables ------------------ |
+     156             :   double state_x_;
+     157             :   double state_y_;
+     158             :   double state_z_;
+     159             :   double state_heading_;
+     160             : 
+     161             :   double speed_x_;
+     162             :   double speed_y_;
+     163             :   double speed_heading_;
+     164             : 
+     165             :   double current_heading_;
+     166             :   double current_vertical_direction_;
+     167             : 
+     168             :   double current_vertical_speed_;
+     169             :   double current_horizontal_speed_;
+     170             : 
+     171             :   double current_horizontal_acceleration_;
+     172             :   double current_vertical_acceleration_;
+     173             : 
+     174             :   std::mutex mutex_state_;
+     175             : 
+     176             :   // | ------------------------ profiler ------------------------ |
+     177             : 
+     178             :   mrs_lib::Profiler profiler_;
+     179             :   bool              _profiler_enabled_ = false;
+     180             : };
+     181             : 
+     182             : //}
+     183             : 
+     184             : // | -------------- tracker's interface routines -------------- |
+     185             : 
+     186             : /* //{ initialize() */
+     187             : 
+     188           1 : bool LineTracker::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     189             :                              std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     190             : 
+     191           1 :   this->common_handlers_  = common_handlers;
+     192           1 :   this->private_handlers_ = private_handlers;
+     193             : 
+     194           1 :   _uav_name_ = common_handlers->uav_name;
+     195             : 
+     196           1 :   nh_ = nh;
+     197             : 
+     198           1 :   ros::Time::waitForValid();
+     199             : 
+     200             :   // --------------------------------------------------------------
+     201             :   // |                     loading parameters                     |
+     202             :   // --------------------------------------------------------------
+     203             : 
+     204             :   // | ---------- loading params using the parent's nh ---------- |
+     205             : 
+     206           2 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     207             : 
+     208           1 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     209             : 
+     210           1 :   if (!param_loader_parent.loadedSuccessfully()) {
+     211           0 :     ROS_ERROR("[LineTracker]: Could not load all parameters!");
+     212           0 :     return false;
+     213             :   }
+     214             : 
+     215             :   // | ---------------- load plugin's parameters ---------------- |
+     216             : 
+     217           1 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/line_tracker.yaml");
+     218           1 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/line_tracker.yaml");
+     219             : 
+     220           2 :   const std::string yaml_prefix = "mrs_uav_trackers/line_tracker/";
+     221             : 
+     222           1 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_speed", _horizontal_speed_);
+     223           1 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_acceleration", _horizontal_acceleration_);
+     224             : 
+     225           1 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     226           1 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_acceleration", _vertical_acceleration_);
+     227             : 
+     228           1 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     229           1 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_gain", _heading_gain_);
+     230             : 
+     231           1 :   private_handlers->param_loader->loadParam(yaml_prefix + "tracker_loop_rate", _tracker_loop_rate_);
+     232             : 
+     233           1 :   _tracker_dt_ = 1.0 / double(_tracker_loop_rate_);
+     234             : 
+     235           1 :   ROS_INFO("[LineTracker]: tracker_dt: %.2f", _tracker_dt_);
+     236             : 
+     237           1 :   state_x_       = 0;
+     238           1 :   state_y_       = 0;
+     239           1 :   state_z_       = 0;
+     240           1 :   state_heading_ = 0;
+     241             : 
+     242           1 :   speed_x_       = 0;
+     243           1 :   speed_y_       = 0;
+     244           1 :   speed_heading_ = 0;
+     245             : 
+     246           1 :   current_horizontal_speed_ = 0;
+     247           1 :   current_vertical_speed_   = 0;
+     248             : 
+     249           1 :   current_horizontal_acceleration_ = 0;
+     250           1 :   current_vertical_acceleration_   = 0;
+     251             : 
+     252           1 :   current_vertical_direction_ = 0;
+     253             : 
+     254           1 :   current_state_vertical_  = IDLE_STATE;
+     255           1 :   previous_state_vertical_ = IDLE_STATE;
+     256             : 
+     257           1 :   current_state_horizontal_  = IDLE_STATE;
+     258           1 :   previous_state_horizontal_ = IDLE_STATE;
+     259             : 
+     260             :   // --------------------------------------------------------------
+     261             :   // |                          profiler                          |
+     262             :   // --------------------------------------------------------------
+     263             : 
+     264           1 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "LineTracker", _profiler_enabled_);
+     265             : 
+     266             :   // --------------------------------------------------------------
+     267             :   // |                           timers                           |
+     268             :   // --------------------------------------------------------------
+     269             : 
+     270           1 :   main_timer_ = nh_.createTimer(ros::Rate(_tracker_loop_rate_), &LineTracker::mainTimer, this);
+     271             : 
+     272           1 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     273           0 :     ROS_ERROR("[LineTracker]: could not load all parameters!");
+     274           0 :     return false;
+     275             :   }
+     276             : 
+     277           1 :   is_initialized_ = true;
+     278             : 
+     279           1 :   ROS_INFO("[LineTracker]: initialized");
+     280             : 
+     281           1 :   return true;
+     282             : }
+     283             : 
+     284             : //}
+     285             : 
+     286             : /* //{ activate() */
+     287             : 
+     288           1 : std::tuple<bool, std::string> LineTracker::activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     289             : 
+     290           2 :   std::stringstream ss;
+     291             : 
+     292           1 :   if (!got_uav_state_) {
+     293             : 
+     294           0 :     ss << "odometry not set";
+     295           0 :     ROS_ERROR_STREAM("[LineTracker]: " << ss.str());
+     296           0 :     return std::tuple(false, ss.str());
+     297             :   }
+     298             : 
+     299             :   // copy member variables
+     300           2 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     301             : 
+     302             :   double uav_heading;
+     303             : 
+     304             :   try {
+     305           1 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     306             :   }
+     307           0 :   catch (...) {
+     308           0 :     ss << "could not calculate the UAV heading";
+     309           0 :     return std::tuple(false, ss.str());
+     310             :   }
+     311             : 
+     312             :   {
+     313           2 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     314             : 
+     315           1 :     if (last_tracker_cmd) {
+     316             : 
+     317             :       // the last command is usable
+     318           1 :       if (last_tracker_cmd->use_position_horizontal) {
+     319           1 :         state_x_ = last_tracker_cmd->position.x;
+     320           1 :         state_y_ = last_tracker_cmd->position.y;
+     321             :       } else {
+     322           0 :         state_x_ = uav_state.pose.position.x;
+     323           0 :         state_y_ = uav_state.pose.position.y;
+     324             :       }
+     325             : 
+     326           1 :       if (last_tracker_cmd->use_position_vertical) {
+     327           1 :         state_z_ = last_tracker_cmd->position.z;
+     328             :       } else {
+     329           0 :         state_z_ = uav_state.pose.position.z;
+     330             :       }
+     331             : 
+     332           1 :       if (last_tracker_cmd->use_heading) {
+     333           1 :         state_heading_ = last_tracker_cmd->heading;
+     334           0 :       } else if (last_tracker_cmd->use_orientation) {
+     335             :         try {
+     336           0 :           state_heading_ = mrs_lib::AttitudeConverter(last_tracker_cmd->orientation).getHeading();
+     337             :         }
+     338           0 :         catch (...) {
+     339           0 :           state_heading_ = uav_heading;
+     340             :         }
+     341             :       } else {
+     342           0 :         state_heading_ = uav_heading;
+     343             :       }
+     344             : 
+     345           1 :       if (last_tracker_cmd->use_velocity_horizontal) {
+     346           1 :         speed_x_ = last_tracker_cmd->velocity.x;
+     347           1 :         speed_y_ = last_tracker_cmd->velocity.y;
+     348             :       } else {
+     349           0 :         speed_x_ = uav_state.velocity.linear.x;
+     350           0 :         speed_y_ = uav_state.velocity.linear.y;
+     351             :       }
+     352             : 
+     353           1 :       current_heading_          = atan2(speed_y_, speed_x_);
+     354           1 :       current_horizontal_speed_ = sqrt(pow(speed_x_, 2) + pow(speed_y_, 2));
+     355             : 
+     356           1 :       current_vertical_speed_     = fabs(last_tracker_cmd->velocity.z);
+     357           1 :       current_vertical_direction_ = last_tracker_cmd->velocity.z > 0 ? +1 : -1;
+     358             : 
+     359           1 :       current_horizontal_acceleration_ = 0;
+     360           1 :       current_vertical_acceleration_   = 0;
+     361             : 
+     362           1 :       goal_heading_ = last_tracker_cmd->heading;
+     363             : 
+     364           1 :       ROS_INFO("[LineTracker]: initial condition: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", last_tracker_cmd->position.x, last_tracker_cmd->position.y,
+     365             :                last_tracker_cmd->position.z, last_tracker_cmd->heading);
+     366           1 :       ROS_INFO("[LineTracker]: initial condition: x_rate=%.2f, y_rate=%.2f, z_rate=%.2f", speed_x_, speed_y_, current_vertical_speed_);
+     367             : 
+     368             :     } else {
+     369             : 
+     370           0 :       state_x_       = uav_state.pose.position.x;
+     371           0 :       state_y_       = uav_state.pose.position.y;
+     372           0 :       state_z_       = uav_state.pose.position.z;
+     373           0 :       state_heading_ = uav_heading;
+     374             : 
+     375           0 :       speed_x_                  = uav_state.velocity.linear.x;
+     376           0 :       speed_y_                  = uav_state.velocity.linear.y;
+     377           0 :       current_heading_          = atan2(speed_y_, speed_x_);
+     378           0 :       current_horizontal_speed_ = sqrt(pow(speed_x_, 2) + pow(speed_y_, 2));
+     379             : 
+     380           0 :       current_vertical_speed_     = fabs(uav_state.velocity.linear.z);
+     381           0 :       current_vertical_direction_ = uav_state.velocity.linear.z > 0 ? +1 : -1;
+     382             : 
+     383           0 :       current_horizontal_acceleration_ = 0;
+     384           0 :       current_vertical_acceleration_   = 0;
+     385             : 
+     386           0 :       goal_heading_ = uav_heading;
+     387             : 
+     388           0 :       ROS_WARN("[LineTracker]: the previous command is not usable for activation, using Odometry instead");
+     389             :     }
+     390             :   }
+     391             : 
+     392             :   // --------------------------------------------------------------
+     393             :   // |          horizontal initial conditions prediction          |
+     394             :   // --------------------------------------------------------------
+     395             : 
+     396             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     397             : 
+     398             :   {
+     399           1 :     std::scoped_lock lock(mutex_state_);
+     400             : 
+     401           1 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     402           1 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     403           1 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     404           1 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     405             :   }
+     406             : 
+     407             :   // --------------------------------------------------------------
+     408             :   // |           vertical initial conditions prediction           |
+     409             :   // --------------------------------------------------------------
+     410             : 
+     411             :   double vertical_t_stop, vertical_stop_dist;
+     412             : 
+     413             :   {
+     414           1 :     std::scoped_lock lock(mutex_state_);
+     415             : 
+     416           1 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     417           1 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     418             :   }
+     419             : 
+     420             :   // --------------------------------------------------------------
+     421             :   // |              heading initial condition  prediction             |
+     422             :   // --------------------------------------------------------------
+     423             : 
+     424             :   {
+     425           2 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     426             : 
+     427           1 :     goal_x_ = state_x_ + stop_dist_x;
+     428           1 :     goal_y_ = state_y_ + stop_dist_y;
+     429           1 :     goal_z_ = state_z_ + vertical_stop_dist;
+     430             : 
+     431           1 :     ROS_INFO("[LineTracker]: setting z goal to %.2f", goal_z_);
+     432             :   }
+     433             : 
+     434           1 :   is_active_ = true;
+     435             : 
+     436           1 :   ss << "activated";
+     437           1 :   ROS_INFO_STREAM("[LineTracker]: " << ss.str());
+     438             : 
+     439           1 :   changeState(STOP_MOTION_STATE);
+     440             : 
+     441           1 :   return std::tuple(true, ss.str());
+     442             : }
+     443             : 
+     444             : //}
+     445             : 
+     446             : /* //{ deactivate() */
+     447             : 
+     448           0 : void LineTracker::deactivate(void) {
+     449             : 
+     450           0 :   is_active_ = false;
+     451             : 
+     452           0 :   ROS_INFO("[LineTracker]: deactivated");
+     453           0 : }
+     454             : 
+     455             : //}
+     456             : 
+     457             : /* //{ resetStatic() */
+     458             : 
+     459           0 : bool LineTracker::resetStatic(void) {
+     460             : 
+     461           0 :   if (!is_initialized_) {
+     462           0 :     ROS_ERROR("[LineTracker]: can not reset, not initialized");
+     463           0 :     return false;
+     464             :   }
+     465             : 
+     466           0 :   if (!is_active_) {
+     467           0 :     ROS_ERROR("[LineTracker]: can not reset, not active");
+     468           0 :     return false;
+     469             :   }
+     470             : 
+     471           0 :   ROS_INFO("[LineTracker]: reseting with no dynamics");
+     472             : 
+     473           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     474             : 
+     475             :   double uav_heading;
+     476             :   try {
+     477           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     478             :   }
+     479           0 :   catch (...) {
+     480           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the UAV heading");
+     481           0 :     return false;
+     482             :   }
+     483             : 
+     484             :   {
+     485           0 :     std::scoped_lock lock(mutex_goal_, mutex_state_, mutex_uav_state_);
+     486             : 
+     487           0 :     state_x_       = uav_state_.pose.position.x;
+     488           0 :     state_y_       = uav_state_.pose.position.y;
+     489           0 :     state_z_       = uav_state_.pose.position.z;
+     490           0 :     state_heading_ = uav_heading;
+     491             : 
+     492           0 :     speed_x_                  = 0;
+     493           0 :     speed_y_                  = 0;
+     494           0 :     current_heading_          = 0;
+     495           0 :     current_horizontal_speed_ = 0;
+     496             : 
+     497           0 :     current_vertical_speed_     = 0;
+     498           0 :     current_vertical_direction_ = 0;
+     499             : 
+     500           0 :     current_horizontal_acceleration_ = 0;
+     501           0 :     current_vertical_acceleration_   = 0;
+     502             : 
+     503           0 :     goal_heading_ = uav_heading;
+     504             :   }
+     505             : 
+     506           0 :   changeState(IDLE_STATE);
+     507             : 
+     508           0 :   return true;
+     509             : }
+     510             : 
+     511             : //}
+     512             : 
+     513             : /* //{ update() */
+     514             : 
+     515        1738 : std::optional<mrs_msgs::TrackerCommand> LineTracker::update(const mrs_msgs::UavState &                                          uav_state,
+     516             :                                                             [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput &last_control_output) {
+     517             : 
+     518        5214 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     519        5214 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LineTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     520             : 
+     521             :   {
+     522        1738 :     std::scoped_lock lock(mutex_uav_state_);
+     523             : 
+     524        1738 :     uav_state_ = uav_state;
+     525        1738 :     uav_x_     = uav_state_.pose.position.x;
+     526        1738 :     uav_y_     = uav_state_.pose.position.y;
+     527        1738 :     uav_z_     = uav_state_.pose.position.z;
+     528             : 
+     529        1738 :     got_uav_state_ = true;
+     530             :   }
+     531             : 
+     532             :   // up to this part the update() method is evaluated even when the tracker is not active
+     533        1738 :   if (!is_active_) {
+     534         102 :     return {};
+     535             :   }
+     536             : 
+     537        3272 :   mrs_msgs::TrackerCommand tracker_cmd;
+     538             : 
+     539        1636 :   tracker_cmd.header.stamp    = ros::Time::now();
+     540        1636 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     541             : 
+     542             :   {
+     543        1636 :     std::scoped_lock lock(mutex_state_);
+     544             : 
+     545        1636 :     tracker_cmd.position.x = state_x_;
+     546        1636 :     tracker_cmd.position.y = state_y_;
+     547        1636 :     tracker_cmd.position.z = state_z_;
+     548        1636 :     tracker_cmd.heading    = radians::wrap(state_heading_);
+     549             : 
+     550        1636 :     tracker_cmd.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     551        1636 :     tracker_cmd.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     552        1636 :     tracker_cmd.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     553        1636 :     tracker_cmd.heading_rate = speed_heading_;
+     554             : 
+     555        1636 :     tracker_cmd.acceleration.x = 0;
+     556        1636 :     tracker_cmd.acceleration.y = 0;
+     557        1636 :     tracker_cmd.acceleration.z = current_vertical_direction_ * current_vertical_acceleration_;
+     558             : 
+     559        1636 :     tracker_cmd.use_position_vertical   = 1;
+     560        1636 :     tracker_cmd.use_position_horizontal = 1;
+     561        1636 :     tracker_cmd.use_heading             = 1;
+     562        1636 :     tracker_cmd.use_heading_rate        = 1;
+     563        1636 :     tracker_cmd.use_velocity_vertical   = 1;
+     564        1636 :     tracker_cmd.use_velocity_horizontal = 1;
+     565        1636 :     tracker_cmd.use_acceleration        = 1;
+     566             :   }
+     567             : 
+     568        1636 :   return {tracker_cmd};
+     569             : }
+     570             : 
+     571             : //}
+     572             : 
+     573             : /* //{ getStatus() */
+     574             : 
+     575         181 : const mrs_msgs::TrackerStatus LineTracker::getStatus() {
+     576             : 
+     577         181 :   mrs_msgs::TrackerStatus tracker_status;
+     578             : 
+     579         181 :   tracker_status.active            = is_active_;
+     580         181 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     581             : 
+     582         181 :   bool idling = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     583             : 
+     584         181 :   tracker_status.have_goal = !idling;
+     585             : 
+     586         181 :   tracker_status.tracking_trajectory = false;
+     587             : 
+     588         181 :   return tracker_status;
+     589             : }
+     590             : 
+     591             : //}
+     592             : 
+     593             : /* //{ enableCallbacks() */
+     594             : 
+     595           0 : const std_srvs::SetBoolResponse::ConstPtr LineTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     596             : 
+     597           0 :   std_srvs::SetBoolResponse res;
+     598           0 :   std::stringstream         ss;
+     599             : 
+     600           0 :   if (cmd->data != callbacks_enabled_) {
+     601             : 
+     602           0 :     callbacks_enabled_ = cmd->data;
+     603             : 
+     604           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     605           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     606             : 
+     607             :   } else {
+     608             : 
+     609           0 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     610           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     611             :   }
+     612             : 
+     613           0 :   res.message = ss.str();
+     614           0 :   res.success = true;
+     615             : 
+     616           0 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     617             : }
+     618             : 
+     619             : //}
+     620             : 
+     621             : /* switchOdometrySource() //{ */
+     622             : 
+     623           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     624             : 
+     625           0 :   std::scoped_lock lock(mutex_goal_, mutex_state_);
+     626             : 
+     627           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     628             : 
+     629           0 :   double old_heading  = 0;
+     630           0 :   double new_heading  = 0;
+     631           0 :   bool   got_headings = true;
+     632             : 
+     633             :   try {
+     634           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     635             :   }
+     636           0 :   catch (...) {
+     637           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+     638           0 :     got_headings = false;
+     639             :   }
+     640             : 
+     641             :   try {
+     642           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+     643             :   }
+     644           0 :   catch (...) {
+     645           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+     646           0 :     got_headings = false;
+     647             :   }
+     648             : 
+     649           0 :   std_srvs::TriggerResponse res;
+     650             : 
+     651           0 :   if (!got_headings) {
+     652           0 :     res.message = "could not calculate the heading difference";
+     653           0 :     res.success = false;
+     654             : 
+     655           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     656             :   }
+     657             : 
+     658             :   // | --------- recalculate the goal to new coordinates -------- |
+     659             : 
+     660           0 :   double dx       = new_uav_state.pose.position.x - uav_state.pose.position.x;
+     661           0 :   double dy       = new_uav_state.pose.position.y - uav_state.pose.position.y;
+     662           0 :   double dz       = new_uav_state.pose.position.z - uav_state.pose.position.z;
+     663           0 :   double dheading = new_heading - old_heading;
+     664             : 
+     665           0 :   goal_x_ += dx;
+     666           0 :   goal_y_ += dy;
+     667           0 :   goal_z_ += dz;
+     668           0 :   goal_heading_ += dheading;
+     669             : 
+     670             :   // | -------------------- update the state -------------------- |
+     671             : 
+     672           0 :   state_x_ += dx;
+     673           0 :   state_y_ += dy;
+     674           0 :   state_z_ += dz;
+     675           0 :   state_heading_ += dheading;
+     676             : 
+     677           0 :   current_heading_ = atan2(goal_y_ - state_y_, goal_x_ - state_x_);
+     678             : 
+     679           0 :   res.message = "odometry source switched";
+     680           0 :   res.success = true;
+     681             : 
+     682           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     683             : }
+     684             : 
+     685             : //}
+     686             : 
+     687             : /* //{ hover() */
+     688             : 
+     689           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     690             : 
+     691           0 :   std_srvs::TriggerResponse res;
+     692             : 
+     693             :   // --------------------------------------------------------------
+     694             :   // |          horizontal initial conditions prediction          |
+     695             :   // --------------------------------------------------------------
+     696             :   {
+     697           0 :     std::scoped_lock lock(mutex_state_, mutex_uav_state_);
+     698             : 
+     699           0 :     current_horizontal_speed_ = sqrt(pow(uav_state_.velocity.linear.x, 2) + pow(uav_state_.velocity.linear.y, 2));
+     700           0 :     current_vertical_speed_   = uav_state_.velocity.linear.z;
+     701           0 :     current_heading_          = atan2(uav_state_.velocity.linear.y, uav_state_.velocity.linear.x);
+     702             :   }
+     703             : 
+     704             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     705             : 
+     706             :   {
+     707           0 :     std::scoped_lock lock(mutex_state_);
+     708             : 
+     709           0 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     710           0 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     711           0 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     712           0 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     713             :   }
+     714             : 
+     715             :   // --------------------------------------------------------------
+     716             :   // |           vertical initial conditions prediction           |
+     717             :   // --------------------------------------------------------------
+     718             : 
+     719             :   double vertical_t_stop, vertical_stop_dist;
+     720             : 
+     721             :   {
+     722           0 :     std::scoped_lock lock(mutex_state_);
+     723             : 
+     724           0 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     725           0 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     726             :   }
+     727             : 
+     728             :   // --------------------------------------------------------------
+     729             :   // |                        set the goal                        |
+     730             :   // --------------------------------------------------------------
+     731             : 
+     732             :   {
+     733           0 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     734             : 
+     735           0 :     goal_x_ = state_x_ + stop_dist_x;
+     736           0 :     goal_y_ = state_y_ + stop_dist_y;
+     737           0 :     goal_z_ = state_z_ + vertical_stop_dist;
+     738             :   }
+     739             : 
+     740           0 :   res.message = "hover initiated";
+     741           0 :   res.success = true;
+     742             : 
+     743           0 :   changeState(STOP_MOTION_STATE);
+     744             : 
+     745           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     746             : }
+     747             : 
+     748             : //}
+     749             : 
+     750             : /* //{ startTrajectoryTracking() */
+     751             : 
+     752           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     753           0 :   return std_srvs::TriggerResponse::Ptr();
+     754             : }
+     755             : 
+     756             : //}
+     757             : 
+     758             : /* //{ stopTrajectoryTracking() */
+     759             : 
+     760           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     761           0 :   return std_srvs::TriggerResponse::Ptr();
+     762             : }
+     763             : 
+     764             : //}
+     765             : 
+     766             : /* //{ resumeTrajectoryTracking() */
+     767             : 
+     768           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     769           0 :   return std_srvs::TriggerResponse::Ptr();
+     770             : }
+     771             : 
+     772             : //}
+     773             : 
+     774             : /* //{ gotoTrajectoryStart() */
+     775             : 
+     776           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     777           0 :   return std_srvs::TriggerResponse::Ptr();
+     778             : }
+     779             : 
+     780             : //}
+     781             : 
+     782             : /* //{ setConstraints() */
+     783             : 
+     784           3 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LineTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     785             : 
+     786           6 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     787             : 
+     788             :   // this is the place to copy the constraints
+     789             :   {
+     790           3 :     std::scoped_lock lock(mutex_constraints_);
+     791             : 
+     792           3 :     _horizontal_speed_        = cmd->constraints.horizontal_speed;
+     793           3 :     _horizontal_acceleration_ = cmd->constraints.horizontal_acceleration;
+     794             : 
+     795           3 :     _vertical_speed_        = cmd->constraints.vertical_ascending_speed;
+     796           3 :     _vertical_acceleration_ = cmd->constraints.vertical_ascending_acceleration;
+     797             : 
+     798           3 :     _heading_rate_ = cmd->constraints.heading_speed;
+     799             :   }
+     800             : 
+     801           3 :   res.success = true;
+     802           3 :   res.message = "constraints updated";
+     803             : 
+     804           6 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     805             : }
+     806             : 
+     807             : //}
+     808             : 
+     809             : /* //{ setReference() */
+     810             : 
+     811           1 : const mrs_msgs::ReferenceSrvResponse::ConstPtr LineTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     812             : 
+     813           2 :   mrs_msgs::ReferenceSrvResponse res;
+     814             : 
+     815           1 :   auto state_heading = mrs_lib::get_mutexed(mutex_state_, state_heading_);
+     816             : 
+     817             :   {
+     818           1 :     std::scoped_lock lock(mutex_goal_);
+     819             : 
+     820           1 :     goal_x_       = cmd->reference.position.x;
+     821           1 :     goal_y_       = cmd->reference.position.y;
+     822           1 :     goal_z_       = cmd->reference.position.z;
+     823           1 :     goal_heading_ = radians::unwrap(cmd->reference.heading, state_heading);
+     824             : 
+     825           1 :     ROS_INFO("[LineTracker]: received new setpoint %.2f, %.2f, %.2f, %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     826             : 
+     827           1 :     have_goal_ = true;
+     828             :   }
+     829             : 
+     830           1 :   changeState(STOP_MOTION_STATE);
+     831             : 
+     832           1 :   res.success = true;
+     833           1 :   res.message = "reference set";
+     834             : 
+     835           2 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+     836             : }
+     837             : 
+     838             : //}
+     839             : 
+     840             : /* //{ setVelocityReference() */
+     841             : 
+     842           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr LineTracker::setVelocityReference([
+     843             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     844           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     845             : }
+     846             : 
+     847             : //}
+     848             : 
+     849             : /* //{ setTrajectoryReference() */
+     850             : 
+     851           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LineTracker::setTrajectoryReference([
+     852             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     853           0 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     854             : }
+     855             : 
+     856             : //}
+     857             : 
+     858             : // | ----------------- state machine routines ----------------- |
+     859             : 
+     860             : /* //{ changeStateHorizontal() */
+     861             : 
+     862           6 : void LineTracker::changeStateHorizontal(States_t new_state) {
+     863             : 
+     864           6 :   previous_state_horizontal_ = current_state_horizontal_;
+     865           6 :   current_state_horizontal_  = new_state;
+     866             : 
+     867             :   // just for ROS_INFO
+     868           6 :   ROS_DEBUG("[LineTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+     869           6 : }
+     870             : 
+     871             : //}
+     872             : 
+     873             : /* //{ changeStateVertical() */
+     874             : 
+     875           6 : void LineTracker::changeStateVertical(States_t new_state) {
+     876             : 
+     877           6 :   previous_state_vertical_ = current_state_vertical_;
+     878           6 :   current_state_vertical_  = new_state;
+     879             : 
+     880             :   // just for ROS_INFO
+     881           6 :   ROS_DEBUG("[LineTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+     882           6 : }
+     883             : 
+     884             : //}
+     885             : 
+     886             : /* //{ changeState() */
+     887             : 
+     888           4 : void LineTracker::changeState(States_t new_state) {
+     889             : 
+     890           4 :   changeStateVertical(new_state);
+     891           4 :   changeStateHorizontal(new_state);
+     892           4 : }
+     893             : 
+     894             : //}
+     895             : 
+     896             : // | --------------------- motion routines -------------------- |
+     897             : 
+     898             : /* //{ stopHorizontalMotion() */
+     899             : 
+     900          13 : void LineTracker::stopHorizontalMotion(void) {
+     901             : 
+     902             :   {
+     903          26 :     std::scoped_lock lock(mutex_state_);
+     904             : 
+     905          13 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     906             : 
+     907          13 :     if (current_horizontal_speed_ < 0) {
+     908          13 :       current_horizontal_speed_        = 0;
+     909          13 :       current_horizontal_acceleration_ = 0;
+     910             :     } else {
+     911           0 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     912             :     }
+     913             :   }
+     914          13 : }
+     915             : 
+     916             : //}
+     917             : 
+     918             : /* //{ stopVerticalMotion() */
+     919             : 
+     920          13 : void LineTracker::stopVerticalMotion(void) {
+     921             : 
+     922             :   {
+     923          26 :     std::scoped_lock lock(mutex_state_);
+     924             : 
+     925          13 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     926             : 
+     927          13 :     if (current_vertical_speed_ < 0) {
+     928           1 :       current_vertical_speed_        = 0;
+     929           1 :       current_vertical_acceleration_ = 0;
+     930             :     } else {
+     931          12 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     932             :     }
+     933             :   }
+     934          13 : }
+     935             : 
+     936             : //}
+     937             : 
+     938             : /* //{ accelerateHorizontal() */
+     939             : 
+     940        1004 : void LineTracker::accelerateHorizontal(void) {
+     941             : 
+     942             :   // copy member variables
+     943        1004 :   auto [goal_x, goal_y]                             = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_);
+     944        1004 :   auto [state_x, state_y, current_horizontal_speed] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, current_horizontal_speed_);
+     945             : 
+     946             :   {
+     947        1004 :     std::scoped_lock lock(mutex_state_);
+     948             : 
+     949        1004 :     current_heading_ = atan2(goal_y - state_y, goal_x - state_x);
+     950             :   }
+     951             : 
+     952        1004 :   auto current_heading = mrs_lib::get_mutexed(mutex_state_, current_heading_);
+     953             : 
+     954             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     955             : 
+     956        1004 :   horizontal_t_stop    = current_horizontal_speed / _horizontal_acceleration_;
+     957        1004 :   horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed) / 2.0;
+     958        1004 :   stop_dist_x          = cos(current_heading) * horizontal_stop_dist;
+     959        1004 :   stop_dist_y          = sin(current_heading) * horizontal_stop_dist;
+     960             : 
+     961             :   {
+     962        2008 :     std::scoped_lock lock(mutex_state_);
+     963             : 
+     964        1004 :     current_horizontal_speed_ += _horizontal_acceleration_ * _tracker_dt_;
+     965             : 
+     966        1004 :     if (current_horizontal_speed_ >= _horizontal_speed_) {
+     967         905 :       current_horizontal_speed_        = _horizontal_speed_;
+     968         905 :       current_horizontal_acceleration_ = 0;
+     969             :     } else {
+     970          99 :       current_horizontal_acceleration_ = _horizontal_acceleration_;
+     971             :     }
+     972             :   }
+     973             : 
+     974        1004 :   if (sqrt(pow(state_x + stop_dist_x - goal_x, 2) + pow(state_y + stop_dist_y - goal_y, 2)) < (2 * (_horizontal_speed_ * _tracker_dt_))) {
+     975             : 
+     976             :     {
+     977           1 :       std::scoped_lock lock(mutex_state_);
+     978             : 
+     979           1 :       current_horizontal_acceleration_ = 0;
+     980             :     }
+     981             : 
+     982           1 :     changeStateHorizontal(DECELERATING_STATE);
+     983             :   }
+     984        1004 : }
+     985             : 
+     986             : //}
+     987             : 
+     988             : /* //{ accelerateVertical() */
+     989             : 
+     990         199 : void LineTracker::accelerateVertical(void) {
+     991             : 
+     992         199 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     993         199 :   auto [state_z, current_vertical_speed] = mrs_lib::get_mutexed(mutex_state_, state_z_, current_vertical_speed_);
+     994             : 
+     995             :   // set the right heading
+     996         199 :   double tar_z = goal_z - state_z;
+     997             : 
+     998             :   // set the right vertical direction
+     999             :   {
+    1000         199 :     std::scoped_lock lock(mutex_state_);
+    1001             : 
+    1002         199 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+    1003             :   }
+    1004             : 
+    1005         199 :   auto current_vertical_direction = mrs_lib::get_mutexed(mutex_state_, current_vertical_direction_);
+    1006             : 
+    1007             :   // calculate the time to stop and the distance it will take to stop [vertical]
+    1008         199 :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+    1009         199 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+    1010         199 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+    1011             : 
+    1012             :   {
+    1013         398 :     std::scoped_lock lock(mutex_state_);
+    1014             : 
+    1015         199 :     current_vertical_speed_ += _vertical_acceleration_ * _tracker_dt_;
+    1016             : 
+    1017         199 :     if (current_vertical_speed_ >= _vertical_speed_) {
+    1018         100 :       current_vertical_speed_        = _vertical_speed_;
+    1019         100 :       current_vertical_acceleration_ = 0;
+    1020             :     } else {
+    1021          99 :       current_vertical_acceleration_ = _vertical_acceleration_;
+    1022             :     }
+    1023             :   }
+    1024             : 
+    1025         199 :   if (fabs(state_z + stop_dist_z - goal_z) < (2 * (_vertical_speed_ * _tracker_dt_))) {
+    1026             : 
+    1027             :     {
+    1028           1 :       std::scoped_lock lock(mutex_state_);
+    1029             : 
+    1030           1 :       current_vertical_acceleration_ = 0;
+    1031             :     }
+    1032             : 
+    1033           1 :     changeStateVertical(DECELERATING_STATE);
+    1034             :   }
+    1035         199 : }
+    1036             : 
+    1037             : //}
+    1038             : 
+    1039             : /* //{ decelerateHorizontal() */
+    1040             : 
+    1041         100 : void LineTracker::decelerateHorizontal(void) {
+    1042             : 
+    1043             :   {
+    1044         200 :     std::scoped_lock lock(mutex_state_);
+    1045             : 
+    1046         100 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+    1047             : 
+    1048         100 :     if (current_horizontal_speed_ < 0) {
+    1049           1 :       current_horizontal_speed_ = 0;
+    1050             :     } else {
+    1051          99 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+    1052             :     }
+    1053             :   }
+    1054             : 
+    1055         100 :   auto current_horizontal_speed = mrs_lib::get_mutexed(mutex_state_, current_horizontal_speed_);
+    1056             : 
+    1057         100 :   if (current_horizontal_speed == 0) {
+    1058             : 
+    1059             :     {
+    1060           1 :       std::scoped_lock lock(mutex_state_);
+    1061             : 
+    1062           1 :       current_horizontal_acceleration_ = 0;
+    1063             :     }
+    1064             : 
+    1065           1 :     changeStateHorizontal(STOPPING_STATE);
+    1066             :   }
+    1067         100 : }
+    1068             : 
+    1069             : //}
+    1070             : 
+    1071             : /* //{ decelerateVertical() */
+    1072             : 
+    1073         100 : void LineTracker::decelerateVertical(void) {
+    1074             : 
+    1075             :   {
+    1076         200 :     std::scoped_lock lock(mutex_state_);
+    1077             : 
+    1078         100 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+    1079             : 
+    1080         100 :     if (current_vertical_speed_ < 0) {
+    1081           1 :       current_vertical_speed_ = 0;
+    1082             :     } else {
+    1083          99 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+    1084             :     }
+    1085             :   }
+    1086             : 
+    1087         100 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1088             : 
+    1089         100 :   if (current_vertical_speed == 0) {
+    1090           1 :     current_vertical_acceleration_ = 0;
+    1091           1 :     changeStateVertical(STOPPING_STATE);
+    1092             :   }
+    1093         100 : }
+    1094             : 
+    1095             : //}
+    1096             : 
+    1097             : /* //{ stopHorizontal() */
+    1098             : 
+    1099          46 : void LineTracker::stopHorizontal(void) {
+    1100             : 
+    1101             :   {
+    1102          46 :     std::scoped_lock lock(mutex_state_);
+    1103             : 
+    1104          46 :     state_x_                         = 0.95 * state_x_ + 0.05 * goal_x_;
+    1105          46 :     state_y_                         = 0.95 * state_y_ + 0.05 * goal_y_;
+    1106          46 :     current_horizontal_acceleration_ = 0;
+    1107             :   }
+    1108          46 : }
+    1109             : 
+    1110             : //}
+    1111             : 
+    1112             : /* //{ stopVertical() */
+    1113             : 
+    1114         851 : void LineTracker::stopVertical(void) {
+    1115             : 
+    1116             :   {
+    1117         851 :     std::scoped_lock lock(mutex_state_);
+    1118             : 
+    1119         851 :     state_z_                       = 0.95 * state_z_ + 0.05 * goal_z_;
+    1120         851 :     current_vertical_acceleration_ = 0;
+    1121             :   }
+    1122         851 : }
+    1123             : 
+    1124             : //}
+    1125             : 
+    1126             : // | ------------------------- timers ------------------------- |
+    1127             : 
+    1128             : /* //{ mainTimer() */
+    1129             : 
+    1130        2234 : void LineTracker::mainTimer(const ros::TimerEvent &event) {
+    1131             : 
+    1132        2234 :   if (!is_active_) {
+    1133         421 :     return;
+    1134             :   }
+    1135             : 
+    1136        5439 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _tracker_loop_rate_, 0.01, event);
+    1137        5439 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("LineTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1138             : 
+    1139        1813 :   auto [goal_x, goal_y, goal_z]    = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1140        1813 :   auto [state_x, state_y, state_z] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, state_z_);
+    1141             : 
+    1142        1813 :   switch (current_state_horizontal_) {
+    1143             : 
+    1144         650 :     case IDLE_STATE:
+    1145             : 
+    1146         650 :       break;
+    1147             : 
+    1148          13 :     case STOP_MOTION_STATE:
+    1149             : 
+    1150          13 :       stopHorizontalMotion();
+    1151             : 
+    1152          13 :       break;
+    1153             : 
+    1154        1004 :     case ACCELERATING_STATE:
+    1155             : 
+    1156        1004 :       accelerateHorizontal();
+    1157             : 
+    1158        1004 :       break;
+    1159             : 
+    1160         100 :     case DECELERATING_STATE:
+    1161             : 
+    1162         100 :       decelerateHorizontal();
+    1163             : 
+    1164         100 :       break;
+    1165             : 
+    1166          46 :     case STOPPING_STATE:
+    1167             : 
+    1168          46 :       stopHorizontal();
+    1169             : 
+    1170          46 :       break;
+    1171             :   }
+    1172             : 
+    1173        1813 :   switch (current_state_vertical_) {
+    1174             : 
+    1175         650 :     case IDLE_STATE:
+    1176             : 
+    1177         650 :       break;
+    1178             : 
+    1179          13 :     case STOP_MOTION_STATE:
+    1180             : 
+    1181          13 :       stopVerticalMotion();
+    1182             : 
+    1183          13 :       break;
+    1184             : 
+    1185         199 :     case ACCELERATING_STATE:
+    1186             : 
+    1187         199 :       accelerateVertical();
+    1188             : 
+    1189         199 :       break;
+    1190             : 
+    1191         100 :     case DECELERATING_STATE:
+    1192             : 
+    1193         100 :       decelerateVertical();
+    1194             : 
+    1195         100 :       break;
+    1196             : 
+    1197         851 :     case STOPPING_STATE:
+    1198             : 
+    1199         851 :       stopVertical();
+    1200             : 
+    1201         851 :       break;
+    1202             :   }
+    1203             : 
+    1204        1813 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1205          13 :     if (current_vertical_speed_ == 0 && current_horizontal_speed_ == 0) {
+    1206           1 :       if (have_goal_) {
+    1207           1 :         changeState(ACCELERATING_STATE);
+    1208             :       } else {
+    1209           0 :         changeState(STOPPING_STATE);
+    1210             :       }
+    1211             :     }
+    1212             :   }
+    1213             : 
+    1214        1813 :   if (current_state_horizontal_ == STOPPING_STATE && current_state_vertical_ == STOPPING_STATE) {
+    1215          47 :     if (fabs(state_x - goal_x) < 1e-3 && fabs(state_y - goal_y) < 1e-3 && fabs(state_z - goal_z) < 1e-3) {
+    1216             : 
+    1217             :       {
+    1218           1 :         std::scoped_lock lock(mutex_state_);
+    1219             : 
+    1220           1 :         state_x_ = goal_x;
+    1221           1 :         state_y_ = goal_y;
+    1222           1 :         state_z_ = goal_z;
+    1223             :       }
+    1224             : 
+    1225           1 :       changeState(IDLE_STATE);
+    1226             : 
+    1227           1 :       have_goal_ = false;
+    1228             :     }
+    1229             :   }
+    1230             : 
+    1231             :   {
+    1232        1813 :     std::scoped_lock lock(mutex_state_);
+    1233             : 
+    1234        1813 :     state_x_ += cos(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1235        1813 :     state_y_ += sin(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1236        1813 :     state_z_ += current_vertical_direction_ * current_vertical_speed_ * _tracker_dt_;
+    1237             :   }
+    1238             : 
+    1239             :   // --------------------------------------------------------------
+    1240             :   // |                        heading tracking                        |
+    1241             :   // --------------------------------------------------------------
+    1242             : 
+    1243             :   {
+    1244        3626 :     std::scoped_lock lock(mutex_state_);
+    1245             : 
+    1246             :     // compute the desired heading rate
+    1247             :     double current_heading_rate;
+    1248        1813 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1249           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1250             :     else
+    1251        1813 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1252             : 
+    1253        1813 :     if (current_heading_rate > _heading_rate_) {
+    1254          50 :       current_heading_rate = _heading_rate_;
+    1255        1763 :     } else if (current_heading_rate < -_heading_rate_) {
+    1256           0 :       current_heading_rate = -_heading_rate_;
+    1257             :     }
+    1258             : 
+    1259             :     // flap the resulted state_heading_ aroud PI
+    1260        1813 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1261             : 
+    1262        1813 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1263        1374 :       state_heading_ = goal_heading_;
+    1264             :     }
+    1265             :   }
+    1266             : }
+    1267             : 
+    1268             : //}
+    1269             : 
+    1270             : }  // namespace line_tracker
+    1271             : 
+    1272             : }  // namespace mrs_uav_trackers
+    1273             : 
+    1274             : #include <pluginlib/class_list_macros.h>
+    1275           1 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::line_tracker::LineTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..b00a836f88 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html @@ -0,0 +1,339 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..0daa33884f86b1f8c9d5c8db38aec9bcd1856369 GIT binary patch literal 3780 zcmV;#4mQIT0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpoi( zl&+^U`rpwAKEXdo7L_!p9fH;Y6a%XEi&#PiSTQ^i5{|c^qT^Y|5n;ezhZG^uQ3zjV zfwl%JVPcAw0+5CU9$?TvN#KNlwRH?AmKY5zBlwvVW{ls8fP)mcBL%?G)KM<96zv%( z+EhX@QB3vnz)>gl4O->cWPkvz-)Oiw1fs@*kUjvkVvVBKeUgv8mY$9K+uQ`-eK+{- zA>odWXsRo$qbP{$02G(A{myVR*$b1@4SHaXwfE^mlDh@I5ow^f0DU~OPIekyrH@d@ zO{5rCHUy4R;3Ni)wQeKM3o!(vBp1gRDNkkXXx38TFQUl9FffIa7hp5MSirESr?^*5 zQcVn$Q{>ew0&=xSK(R0xVB^j~qw6?@GpU-M5zMc90drU%&agwequ4Q=@wp$u6zr#Y zirs*1RIG@#7}&m!S&~yUbhTuiq!+TTSIS5jPI>}Z&srMqNht<%g|J=A=L*@mtozb& zqvOq%0l1;qVcBH>Qu`l6c{2_!W7epS_xo~3WpTe(`0;H$Xc%*y5Q zW+*cn(2kteE`)}QdnTmgl}SU9uG$g;ACDI(+y4Li$MgN)`N6}K6Ziz5Fy;2hAuLec zg_DbdPp@A-wPzbVC<>sWuTPI-ey$;@)dCe1xq_%GfPbtTuwK*@-{S`!Lc#%{u#SuU zX=_(Z@%>_1(xXx@;L$!gps-x0jzFk zTxr!GJUOVN1xGbrs~ou;+*l#UU7wneS4g4 zHrqc=dyF%F+84C;d`Kyz9Wpz6U4j?5-4x~u>~&2vzCz86=Q|Tai9HDOEX$@GtSM6< z>nH`T8UJ(w_kKJb2^!!jHIoBH`Xg5!wjBfcKHNM%AT?EECv_xvm;>1B_%cHcDuI_d zrOvya6KKaI+(ft>KuPsa5@3JrTl(IHNh%u1>()pPJ)`1iaC>lY` zzoQG`KTDBWUG&vRQoMsni6&H#c@%juCiDK49+LV2b6(H5W{J6a)@KPurgEVB&;r9* z5(FOKdx}r{dfWd%EsSryc$_6!L*D1x_XqD5E@o}Ii0B;x_+Wub)>euHP)X4Ua?Dh= zr~Fje!sA1~em%L#anJQ@d^#w2Su#!vo`~r`@Y}~no&tnTt1rbvG#7dg6@GDAgyR6-pmTT?3>)|I{WH@&9%qx zH_O&hD$IK(#M>SM;xnJf57o18URYI1)bE#j?cKC`lWNtS#a z;q0R(m(paCyO49d2sj66J(_iUpiY?V%(+j#XFPYCSew`<118-zocE{f`%wJGw9Fdq zDa<0+gxD+nfHbY;wr{YQ8fO)dGVsfgKV)2$QbTD@J^uhO%*CTtFBFE&u?NZhLLkP5 znK=6mRmT!x5~n&UfXv(CI^Ma{9|Vv-Sb>2?5D`T&uyGxeth>0q#Qt$l@lt8UX4m-Q zPz-ryG?o<}-N9-Z01GIdgqX87)DbA8az(6{V8pVia}=$W9&A`xwX8YXxRb3t8PED- z2)vZdG&38YUjno7=#%HGFmb-6X2T#+ux}LQfpXU8c65-NCiAaFS%oLR)m=%^sPqBF@s)-^;+Iklm6z1GpgPPu_coo8QN|BfbPWFrgwp>G^wljyHT_u;| zSMr;rRAj#ARpNWXRPz=B6%-qKGiLs42s;rP+<${Twj=WrZR^Vf6gue>>A+3Ojoc%br+ zUO@i^yG*0=;d|urA>hl!xGA8p;M?)W30^;WepJ-gdtjJfmhJ<5+5HJE#zG%cJmK%> zE0_{!DVm7%#h;=5i%2sRPmk2_F;KFIbn7x37B~0@4-Hi8`S8tHBsF`^2I%m%g)_}9 z8XG8z$9uNpyNs?MJ5xdQ45^bPJ+lSdXYf1JGybcfE1ypXJenQSfUhBI0kF&c6e=tH zzs^*2RI@a6(ue+Jk+mtbo#i$D_l5Xgmr}bQmFQ`FD|w(rPWq@#-&6znU%W~!kZYNf}-6j z9q+>jI{Y(RfonHF&xS_&Vx?}tJL65jw!4~>@#{%w0spJ6U;@VwGQhr@-MNmQ^}Fn} zZZ9Ev3%7?m|6*>hmFuJ9&!aHO)Q$g>{VR5A{|XQ9UMzI&I3=Z8T-4y_HVMALQHx)s z!To+yzlh|L$u&>>YBr7I9Ll-1+53^wP=UVVLY=spQnXs(CzK*m2%XgO*eqns9n-o-&p>O9 z9;jJ8Dzu2CXa^9VX}#V#&EU9>^F>;UcvxHM^S;r|#WV9PD$T9Hq6?sB`aV+h)&xlH zukgHv6+)Y9k#`|&hUls5*$g%rU@;Xo!@&3ccbi6EEao{@78Zz~Cb;yQkVUEBLMZH&93YcKm`;ug#swxyR()7Pzc0&+JT zhc{KA#|Sew9PjU35^%3Cpa_wxX>c)6+&tf)W@qPi2)V%91p|qYix8_r$VE~ND>HW) zJ`@_h1a&)p*#~L3UkWvoR&o~VnMNt(T__bs+K%KRJGDL|b)=Vz0KPP8Ha9h(590x6 z!2>&OuZJlpA7JXv|D>cg3ptA$cUfVISdXcJ?^{NN;4%#a>=}H-yn&Ru2A}-tzR~6w z2XiCXU=NGnJ2W6)9AegQ*g;P&}1 z2o+GEuz1ZhuPz!Y@LebsM%s>Ks6es)wV}edCJVmA?R_>WP2Im{fUPDzcTZ6!Ob>4s z<{zUfA3rsLChoLUMpG#DH~GiUPNm2?{#yU|rBkVI@sA%}fZ*KYHP#ix1^Y+VJr?^% z);&h^LiQe3(o@jDU+O8E9XW~fA$3xJ+*81h_DQmRRG3e%Xn}c!{HUK3Xyy6(^BYfa^>9$6jBFXm5J$GKf0Q;SQNIdyse;+*g z;@}de!|%$2h;EJjPF+=RzK_?~nHA!iqY|~gXLb=%7AU4TXQ^o^!W42Fio~t*4JgWh zQi?e@uc!DY_4iBNisGCXxqzYs*z+L=DX;hE_VTPHmhe4Nw|4-4Qdd%x0i_iCxxL&m zRvkqNu!GyF-M`7_ z%{}*yR1uV#;;Esn6 zZ1pF2Ops5}q9M-ZQR2Hd@Ch+@Hq5^8SIZu!_eJB(3_XQq?bOWGMR&c?5|fNaQJOXF z!&lB^@N>@S=7|Sf#Q{2b@n@D@PM%83``e$^So)xO3K|@AoqGv3eT5y(QEctQ+xz + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
<unnamed>72.8 %67 / 9250.0 %9 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..93f7cb233d --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
<unnamed>72.8 %67 / 9250.0 %9 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/index-detail.html b/mrs_uav_trackers/src/midair_activation_tracker/index-detail.html new file mode 100644 index 0000000000..79ec9550ac --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
<unnamed>72.8 %67 / 9250.0 %9 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html new file mode 100644 index 0000000000..0d2ed73598 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html new file mode 100644 index 0000000000..664419213f --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/index.html b/mrs_uav_trackers/src/midair_activation_tracker/index.html new file mode 100644 index 0000000000..0efd833619 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_tracker.cpp +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..8c7968b08c --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::resetStatic()0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()38
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)132
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()152
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html new file mode 100644 index 0000000000..03433429b8 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()152
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::resetStatic()0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)132
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()38
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.frameset.html b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.frameset.html new file mode 100644 index 0000000000..fa5ac7f267 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html new file mode 100644 index 0000000000..253a365c42 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html @@ -0,0 +1,426 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-25 05:42:57Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : 
+       8             : #include <mrs_lib/profiler.h>
+       9             : #include <mrs_lib/mutex.h>
+      10             : #include <mrs_lib/attitude_converter.h>
+      11             : #include <mrs_lib/geometry/cyclic.h>
+      12             : #include <mrs_lib/geometry/misc.h>
+      13             : 
+      14             : //}
+      15             : 
+      16             : namespace mrs_uav_trackers
+      17             : {
+      18             : 
+      19             : namespace midair_activation_tracker
+      20             : {
+      21             : 
+      22             : /* //{ class MidairActivationTracker */
+      23             : 
+      24             : class MidairActivationTracker : public mrs_uav_managers::Tracker {
+      25             : public:
+      26             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      27             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      28             : 
+      29             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd);
+      30             :   void                          deactivate(void);
+      31             :   bool                          resetStatic(void);
+      32             : 
+      33             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState &uav_state, const mrs_uav_managers::Controller::ControlOutput &last_control_output);
+      34             :   const mrs_msgs::TrackerStatus             getStatus();
+      35             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd);
+      36             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      37             : 
+      38             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd);
+      39             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd);
+      40             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd);
+      41             : 
+      42             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      43             : 
+      44             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      45             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      46             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      47             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      48             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      49             : 
+      50             : private:
+      51             :   ros::NodeHandle nh_;
+      52             : 
+      53             :   bool callbacks_enabled_ = true;
+      54             : 
+      55             :   std::string _uav_name_;
+      56             : 
+      57             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      58             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      59             : 
+      60             :   // | ---------------- the tracker's inner state --------------- |
+      61             : 
+      62             :   bool is_initialized_ = false;
+      63             :   bool is_active_      = false;
+      64             : 
+      65             :   // | ------------------------ profiler ------------------------ |
+      66             : 
+      67             :   mrs_lib::Profiler profiler_;
+      68             :   bool              _profiler_enabled_ = false;
+      69             : };
+      70             : 
+      71             : //}
+      72             : 
+      73             : // | -------------- tracker's interface routines -------------- |
+      74             : 
+      75             : /* //{ initialize() */
+      76             : 
+      77          94 : bool MidairActivationTracker::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      78             :                                          std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+      79             : 
+      80          94 :   this->common_handlers_  = common_handlers;
+      81          94 :   this->private_handlers_ = private_handlers;
+      82             : 
+      83          94 :   _uav_name_ = common_handlers->uav_name;
+      84             : 
+      85          94 :   nh_ = nh;
+      86             : 
+      87          94 :   ros::Time::waitForValid();
+      88             : 
+      89             :   // --------------------------------------------------------------
+      90             :   // |                     loading parameters                     |
+      91             :   // --------------------------------------------------------------
+      92             : 
+      93             :   // | ---------- loading params using the parent's nh ---------- |
+      94             : 
+      95         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+      96             : 
+      97          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+      98             : 
+      99          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     100           0 :     ROS_ERROR("[MidairActivationTracker]: Could not load all parameters!");
+     101           0 :     return false;
+     102             :   }
+     103             : 
+     104             :   // | ---------------- load plugin's parameters ---------------- |
+     105             : 
+     106          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/midair_activation_tracker.yaml");
+     107          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/midair_activation_tracker.yaml");
+     108             : 
+     109         188 :   const std::string yaml_prefix = "mrs_uav_trackers/midair_activation_tracker/";
+     110             : 
+     111          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     112           0 :     ROS_ERROR("[MidairActivationTracker]: could not load all parameters!");
+     113           0 :     return false;
+     114             :   }
+     115             : 
+     116             :   // | ------------------------ profiler ------------------------ |
+     117             : 
+     118          94 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationTracker", _profiler_enabled_);
+     119             : 
+     120             :   // | --------------------- finish the init -------------------- |
+     121             : 
+     122          94 :   is_initialized_ = true;
+     123             : 
+     124          94 :   ROS_INFO("[MidairActivationTracker]: initialized");
+     125             : 
+     126          94 :   return true;
+     127             : }
+     128             : 
+     129             : //}
+     130             : 
+     131             : /* //{ activate() */
+     132             : 
+     133         132 : std::tuple<bool, std::string> MidairActivationTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     134             : 
+     135         132 :   std::stringstream ss;
+     136             : 
+     137         132 :   is_active_ = true;
+     138             : 
+     139         132 :   ss << "activated";
+     140         132 :   ROS_INFO_STREAM("[MidairActivationTracker]: " << ss.str());
+     141             : 
+     142         264 :   return std::tuple(true, ss.str());
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* //{ deactivate() */
+     148             : 
+     149         152 : void MidairActivationTracker::deactivate(void) {
+     150             : 
+     151         152 :   is_active_ = false;
+     152             : 
+     153         152 :   ROS_INFO("[MidairActivationTracker]: deactivated");
+     154         152 : }
+     155             : 
+     156             : //}
+     157             : 
+     158             : /* //{ resetStatic() */
+     159             : 
+     160           0 : bool MidairActivationTracker::resetStatic(void) {
+     161             : 
+     162           0 :   return false;
+     163             : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* //{ update() */
+     168             : 
+     169      127134 : std::optional<mrs_msgs::TrackerCommand> MidairActivationTracker::update(
+     170             :     const mrs_msgs::UavState &uav_state, [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput &last_control_output) {
+     171             : 
+     172             :   // up to this part the update() method is evaluated even when the tracker is not active
+     173      127134 :   if (!is_active_) {
+     174      126729 :     return {};
+     175             :   }
+     176             : 
+     177        1215 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     178             :   mrs_lib::ScopeTimer timer =
+     179        1215 :       mrs_lib::ScopeTimer("MidairActivationTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     180             : 
+     181         810 :   mrs_msgs::TrackerCommand tracker_cmd;
+     182             : 
+     183         405 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     184         405 :   tracker_cmd.header.stamp    = ros::Time::now();
+     185             : 
+     186         405 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     187         405 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     188         405 :   tracker_cmd.position.z = uav_state.pose.position.z;
+     189             : 
+     190         405 :   tracker_cmd.velocity.x = uav_state.velocity.linear.x;
+     191         405 :   tracker_cmd.velocity.y = uav_state.velocity.linear.y;
+     192         405 :   tracker_cmd.velocity.z = uav_state.velocity.linear.z;
+     193             : 
+     194             :   try {
+     195         405 :     tracker_cmd.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     196             :   }
+     197           0 :   catch (...) {
+     198           0 :     tracker_cmd.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYaw();
+     199           0 :     ROS_WARN_THROTTLE(1.0, "[MidairActivationTracker]: could not get heading");
+     200             :   }
+     201             : 
+     202         405 :   tracker_cmd.use_position_vertical   = true;
+     203         405 :   tracker_cmd.use_position_horizontal = true;
+     204             : 
+     205         405 :   tracker_cmd.use_velocity_vertical   = true;
+     206         405 :   tracker_cmd.use_velocity_horizontal = true;
+     207             : 
+     208         405 :   tracker_cmd.use_heading = true;
+     209             : 
+     210         405 :   ROS_WARN_THROTTLE(0.1, "[MidairActivationTracker]: outputting cmd");
+     211             : 
+     212         405 :   return {tracker_cmd};
+     213             : }
+     214             : 
+     215             : //}
+     216             : 
+     217             : /* //{ getStatus() */
+     218             : 
+     219          38 : const mrs_msgs::TrackerStatus MidairActivationTracker::getStatus() {
+     220             : 
+     221          38 :   mrs_msgs::TrackerStatus tracker_status;
+     222             : 
+     223          38 :   tracker_status.active            = is_active_;
+     224          38 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     225             : 
+     226          38 :   return tracker_status;
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* //{ enableCallbacks() */
+     232             : 
+     233         297 : const std_srvs::SetBoolResponse::ConstPtr MidairActivationTracker::enableCallbacks([[maybe_unused]] const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     234             : 
+     235         594 :   std_srvs::SetBoolResponse res;
+     236             : 
+     237         297 :   res.message = "callbacks are always disabled";
+     238         297 :   res.success = true;
+     239             : 
+     240         594 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : /* switchOdometrySource() //{ */
+     246             : 
+     247           0 : const std_srvs::TriggerResponse::ConstPtr MidairActivationTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     248             : 
+     249           0 :   return std_srvs::TriggerResponse::Ptr();
+     250             : }
+     251             : 
+     252             : //}
+     253             : 
+     254             : /* //{ hover() */
+     255             : 
+     256           0 : const std_srvs::TriggerResponse::ConstPtr MidairActivationTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     257             : 
+     258           0 :   return std_srvs::TriggerResponse::Ptr();
+     259             : }
+     260             : 
+     261             : //}
+     262             : 
+     263             : /* //{ startTrajectoryTracking() */
+     264             : 
+     265           0 : const std_srvs::TriggerResponse::ConstPtr MidairActivationTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     266           0 :   return std_srvs::TriggerResponse::Ptr();
+     267             : }
+     268             : 
+     269             : //}
+     270             : 
+     271             : /* //{ stopTrajectoryTracking() */
+     272             : 
+     273           0 : const std_srvs::TriggerResponse::ConstPtr MidairActivationTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     274           0 :   return std_srvs::TriggerResponse::Ptr();
+     275             : }
+     276             : 
+     277             : //}
+     278             : 
+     279             : /* //{ resumeTrajectoryTracking() */
+     280             : 
+     281           0 : const std_srvs::TriggerResponse::ConstPtr MidairActivationTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     282           0 :   return std_srvs::TriggerResponse::Ptr();
+     283             : }
+     284             : 
+     285             : //}
+     286             : 
+     287             : /* //{ gotoTrajectoryStart() */
+     288             : 
+     289           0 : const std_srvs::TriggerResponse::ConstPtr MidairActivationTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     290           0 :   return std_srvs::TriggerResponse::Ptr();
+     291             : }
+     292             : 
+     293             : //}
+     294             : 
+     295             : /* //{ setConstraints() */
+     296             : 
+     297         325 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationTracker::setConstraints([
+     298             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     299             : 
+     300         650 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     301             : 
+     302         325 :   res.success = true;
+     303         325 :   res.message = "constraints updated";
+     304             : 
+     305         650 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     306             : }
+     307             : 
+     308             : //}
+     309             : 
+     310             : /* //{ setReference() */
+     311             : 
+     312           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MidairActivationTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     313             : 
+     314           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     315             : }
+     316             : 
+     317             : //}
+     318             : 
+     319             : /* //{ setVelocityReference() */
+     320             : 
+     321           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MidairActivationTracker::setVelocityReference([
+     322             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     323           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     324             : }
+     325             : 
+     326             : //}
+     327             : 
+     328             : /* //{ setTrajectoryReference() */
+     329             : 
+     330           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MidairActivationTracker::setTrajectoryReference([
+     331             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     332           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     333             : }
+     334             : 
+     335             : //}
+     336             : 
+     337             : }  // namespace midair_activation_tracker
+     338             : 
+     339             : }  // namespace mrs_uav_trackers
+     340             : 
+     341             : #include <pluginlib/class_list_macros.h>
+     342          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..92abe9ea3b --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.overview.html @@ -0,0 +1,106 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.png b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4f71e52c409a351c83f61f35774f7a90c3d741d3 GIT binary patch literal 1153 zcmV-{1b+L8P)J!}>NZyyuFasP|62uJU5*aFu zjmS0d?pPuUM>KcvwhD9>G)*Rz=p=yIWPvvr_@@Y55pdMTNMg%6%JYb>a|plxWB~;i zsF4hyv^Mf5%|!6`TAz(e^Ad~0QdkGdHqTsTA!J->1xS#7+h-UE z*P@GUim)73fPB?L#8jXQJ(B^lO1%0Hy*7GM^Lkq^aejm3Xq5Ox^wTJOU7cj(=d*u^ zvHRc3yjeD4P`C0`Z{V&?`FF`cK;?mo05XS_xFh>hd2JFe%P0|pHs)z*m$(ouAWnKA z&Vrl78WM~f0LYZc9(K`4`~U8>&3;z&EgHZh8S3tOuV-R8A(&bu(x&Y(@Hl>xaq559 z%g^;=E*yse{@m}2V>xOF2W)oX>USL`>-&o%E-a1)V1uts#<8zzg2Y@HVvoPg*Kv6p z@pX=7aJeb%0h5I7(8Q>X*344QA4(}UN>l`Fk+_%Yh66H+mI1QI6P=XJ{?pn%%yMp3 zcU6T-AJEgK)PQ`6yPYaab+Y`F;Y$G^Uv;RDs$jya;_>-DJms;4a1%XcMjuF0Y!|Xs zKj^~Y^IJE5T}_Tkc3gB9UJ=$nUR^&HW?gS6?2i#X@u?J*q2dcR%-|>|mggwRMhYbW&05|Ly{-x8L`mb* zz31AUMYQS;iE+o71#VmR0HRNI=@8-x;JA%g;s4nf~I@b)c)wInYn^?NK z=JKZERbBI}j%&dMpIF(uQ}uJ5)$C6qGwtFQ4vE` zht6!K@}##)v_)&z-lR9r0625IM{P>uYj24ja7sC26LL2I>G3R8*qQbENDBWoDBz2> zd(%?=m++cIfgi?=GtVddALemFs`&CahsZCFbMonb&*P*s$kKBo*d&GH2UWO5^?r8- z=uA4Xo<_6QKf;dG%rgUQQC)Z@9*$>U(rAjQQARFTYGDrcJ;ZZ{x2@|*q-O|ENDoJL zXa1J+KC+_;Pe^|u;WHSYpGZq71!j>~q!cN_cSk+zQzeeuXun#61_{jPc|86AD!MUP TVQf$600000NkvXXu0mjfd_WxL literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..a344fc6d51 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
<unnamed>82.1 %1361 / 165788.0 %44 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..5b1670ae8d --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
<unnamed>82.1 %1361 / 165788.0 %44 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-detail.html b/mrs_uav_trackers/src/mpc_tracker/index-detail.html new file mode 100644 index 0000000000..c7d8974f23 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
<unnamed>82.1 %1361 / 165788.0 %44 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-sort-f.html b/mrs_uav_trackers/src/mpc_tracker/index-sort-f.html new file mode 100644 index 0000000000..82d4c1e948 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-sort-l.html b/mrs_uav_trackers/src/mpc_tracker/index-sort-l.html new file mode 100644 index 0000000000..4897c6bcb5 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index.html b/mrs_uav_trackers/src/mpc_tracker/index.html new file mode 100644 index 0000000000..3d090785ad --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.1%82.1%
+ 		82.1 %1361 / 165788.0 %44 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..bffe8916eb --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html @@ -0,0 +1,280 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_tracker - mpc_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackWiggle(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintMPCResult(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()40
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)85
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::mpc_tracker::MpcTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)94
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)247
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)249
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)331
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)404
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)489
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)720
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)734
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)737
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)738
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)1173
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1955
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)2138
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)4054
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()8213
mrs_uav_trackers::mpc_tracker::MpcTracker::getCurrentTrajectoryIdx()8214
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)15372
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)20428
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()21949
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)70897
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)76093
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()77111
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)77111
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceXY(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)77111
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()77111
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)77111
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollision(double, double, double, double, double, double)187240
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)187240
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html new file mode 100644 index 0000000000..752a4660b5 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html @@ -0,0 +1,280 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_tracker - mpc_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()40
mrs_uav_trackers::mpc_tracker::MpcTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)404
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)1173
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()77111
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)70897
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)247
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackWiggle(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollision(double, double, double, double, double, double)187240
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)737
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1955
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)489
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)77111
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)20428
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceXY(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)77111
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()77111
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()21949
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintMPCResult(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)720
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)734
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)187240
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::getCurrentTrajectoryIdx()8214
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)738
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)4054
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)331
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)94
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)2138
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)76093
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)15372
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)249
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)85
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)77111
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()8213
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.frameset.html b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.frameset.html new file mode 100644 index 0000000000..70888f86ff --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html new file mode 100644 index 0000000000..13676117d0 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html @@ -0,0 +1,3945 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_tracker - mpc_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1361165782.1 %
Date:2024-04-25 05:42:57Functions:445088.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : #include <mrs_uav_managers/controller.h>
+       8             : 
+       9             : #include <geometry_msgs/Pose.h>
+      10             : #include <geometry_msgs/PoseArray.h>
+      11             : 
+      12             : #include <mrs_msgs/FuturePoint.h>
+      13             : #include <mrs_msgs/FutureTrajectory.h>
+      14             : #include <mrs_msgs/MpcTrackerDiagnostics.h>
+      15             : #include <mrs_msgs/MpcPredictionFullState.h>
+      16             : #include <mrs_msgs/EstimationDiagnostics.h>
+      17             : #include <mrs_msgs/VelocityReference.h>
+      18             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      19             : 
+      20             : #include <std_msgs/String.h>
+      21             : 
+      22             : #include <mrs_lib/profiler.h>
+      23             : #include <mrs_lib/utils.h>
+      24             : #include <mrs_lib/mutex.h>
+      25             : #include <mrs_lib/attitude_converter.h>
+      26             : #include <mrs_lib/subscribe_handler.h>
+      27             : #include <mrs_lib/publisher_handler.h>
+      28             : #include <mrs_lib/geometry/cyclic.h>
+      29             : #include <mrs_lib/geometry/misc.h>
+      30             : #include <mrs_lib/scope_timer.h>
+      31             : 
+      32             : #include <mpc_tracker.h>
+      33             : 
+      34             : #include <dynamic_reconfigure/server.h>
+      35             : #include <mrs_uav_trackers/mpc_trackerConfig.h>
+      36             : 
+      37             : #include <visualization_msgs/Marker.h>
+      38             : #include <visualization_msgs/MarkerArray.h>
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* defines //{ */
+      43             : 
+      44             : #define MPC_N_STATES 12
+      45             : #define MPC_N_INPUTS 3
+      46             : 
+      47             : #define MPC_HEADING_N_STATES 4
+      48             : #define MPC_HEADING_N_INPUTS 1
+      49             : 
+      50             : #define MPC_HORIZON_LENGTH 40
+      51             : 
+      52             : //}
+      53             : 
+      54             : /* using //{ */
+      55             : 
+      56             : using namespace Eigen;
+      57             : 
+      58             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      59             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      60             : 
+      61             : using radians  = mrs_lib::geometry::radians;
+      62             : using sradians = mrs_lib::geometry::sradians;
+      63             : 
+      64             : using quat_t = Eigen::Quaterniond;
+      65             : 
+      66             : //}
+      67             : 
+      68             : namespace mrs_uav_trackers
+      69             : {
+      70             : 
+      71             : namespace mpc_tracker
+      72             : {
+      73             : 
+      74             : /* //{ class MpcTracker */
+      75             : 
+      76             : class MpcTracker : public mrs_uav_managers::Tracker {
+      77             : public:
+      78             :   bool initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      79             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      80             : 
+      81             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd);
+      82             :   void                          deactivate(void);
+      83             :   bool                          resetStatic(void);
+      84             : 
+      85             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState& uav_state, const mrs_uav_managers::Controller::ControlOutput& last_control_output);
+      86             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd);
+      87             :   const mrs_msgs::TrackerStatus             getStatus();
+      88             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState& new_uav_state);
+      89             : 
+      90             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd);
+      91             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd);
+      92             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd);
+      93             : 
+      94             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      95             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      96             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      97             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      98             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      99             : 
+     100             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd);
+     101             : 
+     102             : private:
+     103             :   ros::NodeHandle nh_;
+     104             : 
+     105             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+     106             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+     107             : 
+     108             :   std::atomic<bool> callbacks_enabled_ = true;
+     109             : 
+     110             :   std::string _uav_name_;
+     111             : 
+     112             :   // debugging publishers
+     113             :   mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics> pub_diagnostics_;
+     114             :   mrs_lib::PublisherHandler<std_msgs::String>                pub_status_string_;
+     115             : 
+     116             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_processed_trajectory_poses_;
+     117             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_processed_trajectory_markers_;
+     118             : 
+     119             :   mrs_msgs::UavState uav_state_;
+     120             :   std::mutex         mutex_uav_state_;
+     121             : 
+     122             :   bool is_active_      = false;
+     123             :   bool is_initialized_ = false;
+     124             : 
+     125             :   // | ----------------------- constraints ---------------------- |
+     126             : 
+     127             :   mrs_msgs::DynamicsConstraints constraints_;
+     128             :   std::mutex                    mutex_constraints_;
+     129             : 
+     130             :   mrs_msgs::DynamicsConstraints constraints_filtered_;
+     131             :   std::mutex                    mutex_constraints_filtered_;
+     132             : 
+     133             :   std::atomic<bool> got_constraints_     = false;
+     134             :   std::atomic<bool> all_constraints_set_ = false;
+     135             : 
+     136             :   double _diag_pos_tracking_thr_;
+     137             :   double _diag_heading_tracking_thr_;
+     138             : 
+     139             :   double _mpc_synchronous_rate_limit_;
+     140             :   double _mpc_asynchronous_rate_;
+     141             : 
+     142             :   double update_rate_ = 100.0;
+     143             : 
+     144             :   double     dt1_;
+     145             :   std::mutex mutex_dt1_;
+     146             : 
+     147             :   double _dt2_;
+     148             : 
+     149             :   MatrixXd          _mat_A_;  // system matrix for virtual UAV
+     150             :   MatrixXd          _mat_B_;  // input matrix for virtual UAV
+     151             :   MatrixXd          A_;       // system matrix for virtual UAV
+     152             :   MatrixXd          B_;       // input matrix for virtual UAV
+     153             :   std::atomic<bool> model_first_iteration_ = true;
+     154             :   ros::Time         model_iteration_last_time_;
+     155             : 
+     156             :   MatrixXd _mat_A_heading_;  // system matrix for heading
+     157             :   MatrixXd _mat_B_heading_;  // input matrix for heading
+     158             :   MatrixXd A_heading_;       // system matrix for heading
+     159             :   MatrixXd B_heading_;       // input matrix for heading
+     160             : 
+     161             :   // the reference over the prediction horizon per axis
+     162             :   MatrixXd   des_x_trajectory_;
+     163             :   MatrixXd   des_y_trajectory_;
+     164             :   MatrixXd   des_z_trajectory_;
+     165             :   MatrixXd   des_heading_trajectory_;
+     166             :   std::mutex mutex_des_trajectory_;
+     167             : 
+     168             :   // the reference filtered over the prediction horizon per axis
+     169             :   MatrixXd des_z_filtered_offset_;
+     170             : 
+     171             :   // the whole trajectory reference split per axis
+     172             :   std::shared_ptr<VectorXd> des_x_whole_trajectory_;
+     173             :   std::shared_ptr<VectorXd> des_y_whole_trajectory_;
+     174             :   std::shared_ptr<VectorXd> des_z_whole_trajectory_;
+     175             :   std::shared_ptr<VectorXd> des_heading_whole_trajectory_;
+     176             :   int                       des_whole_trajectory_id_ = 0;
+     177             :   std::mutex                mutex_des_whole_trajectory_;
+     178             : 
+     179             :   int  getCurrentTrajectoryIdx();
+     180             :   void increaseCurrentTrajectoryTime(const double dt);
+     181             : 
+     182             :   // trajectory tracking
+     183             :   std::atomic<bool> trajectory_tracking_in_progress_ = false;
+     184             :   double            trajectory_current_time_;
+     185             :   std::mutex        mutex_trajectory_tracking_states_;
+     186             : 
+     187             :   // params of the loaded trajectory
+     188             :   int    trajectory_size_          = 0;
+     189             :   double trajectory_dt_            = 0.2;
+     190             :   bool   trajectory_track_heading_ = false;
+     191             :   bool   trajectory_tracking_loop_ = false;
+     192             :   bool   trajectory_set_           = false;
+     193             :   int    trajectory_count_         = 0;  // counts how many trajectories we have received
+     194             : 
+     195             :   // mpc output
+     196             :   VectorXd   mpc_u_;
+     197             :   double     mpc_u_heading_;
+     198             :   std::mutex mutex_mpc_u_;
+     199             : 
+     200             :   // current state of the dynamical system
+     201             :   MatrixXd   mpc_x_;          // translation state
+     202             :   MatrixXd   mpc_x_heading_;  // heading state
+     203             :   std::mutex mutex_mpc_x_;
+     204             : 
+     205             :   // odometry reset
+     206             :   std::atomic<bool> odometry_reset_in_progress_ = false;
+     207             :   std::atomic<bool> mpc_result_invalid_         = false;
+     208             : 
+     209             :   // predicting the future
+     210             :   MatrixXd   predicted_trajectory_;
+     211             :   MatrixXd   predicted_heading_trajectory_;
+     212             :   std::mutex mutex_predicted_trajectory_;
+     213             : 
+     214             :   mrs_msgs::MpcPredictionFullState prediction_full_state_;
+     215             :   std::mutex                       mutex_prediction_full_state_;
+     216             : 
+     217             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_predicted_trajectory_debugging_;
+     218             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_mpc_reference_debugging_;
+     219             :   mrs_lib::PublisherHandler<geometry_msgs::PoseStamped> ph_current_trajectory_point_;
+     220             : 
+     221             :   std::atomic<bool> mpc_computed_ = false;
+     222             : 
+     223             :   bool brake_ = false;
+     224             : 
+     225             :   // | ----------------------- MPC solver ----------------------- |
+     226             : 
+     227             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_y_;
+     228             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_x_;
+     229             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_z_;
+     230             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_heading_;
+     231             : 
+     232             :   std::mutex mutex_mpc_calculation_;
+     233             : 
+     234             :   int _max_iters_xy_;
+     235             :   int _max_iters_z_;
+     236             :   int _max_iters_heading_;
+     237             : 
+     238             :   // | ----------- measuring the "MPC realtime factor" ---------- |
+     239             : 
+     240             :   double mpc_rtf_ = 0.0;
+     241             : 
+     242             :   // | ------------------- collision avoidance ------------------ |
+     243             : 
+     244             :   // configurable params
+     245             :   bool collision_avoidance_enabled_           = false;
+     246             :   bool collision_avoidance_enabled_passively_ = true;
+     247             : 
+     248             :   // TODO what is this?
+     249             :   double    coef_scaler = 0;
+     250             :   ros::Time coef_time;
+     251             : 
+     252             :   double minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+     253             : 
+     254             :   // params
+     255             :   double                   _avoidance_trajectory_rate_;
+     256             :   double                   _avoidance_radius_threshold_;
+     257             :   double                   _avoidance_z_correction_;
+     258             :   std::string              _avoidance_diagnostics_topic_name_;
+     259             :   std::vector<std::string> _avoidance_other_uav_names_;
+     260             :   double                   _avoidance_z_threshold_;
+     261             : 
+     262             :   // how old can the other UAV trajectory be (since receive time)
+     263             :   double _collision_trajectory_timeout_;
+     264             : 
+     265             :   // when collision detected, slow down during the manouver
+     266             :   double _avoidance_collision_horizontal_speed_coef_;
+     267             : 
+     268             :   // when collision detected, slow down fully this number of steps before it
+     269             :   int _avoidance_collision_slow_down_fully_;
+     270             : 
+     271             :   // when collision detected, start slowing down this number of steps before it
+     272             :   int _avoidance_collision_slow_down_;
+     273             : 
+     274             :   // when avoiding, start climbing this number of steps before it
+     275             :   int _avoidance_collision_start_climbing_;
+     276             : 
+     277             :   int avoidance_this_uav_number_;
+     278             :   int avoidance_this_uav_priority_;
+     279             : 
+     280             :   double            collision_free_altitude_;
+     281             :   std::atomic<bool> avoiding_collision_               = false;
+     282             :   bool              collision_avoidance_affecting_me_ = false;
+     283             : 
+     284             :   // avoidance trajectory will not be published unless we computed it at least once
+     285             :   std::atomic<bool> future_was_predicted_ = false;
+     286             : 
+     287             :   // subscribing to the other UAV future trajectories
+     288             :   void callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg);
+     289             : 
+     290             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>> other_uav_trajectory_subscribers_;
+     291             :   std::map<std::string, mrs_msgs::FutureTrajectory>                  other_uav_avoidance_trajectories_;
+     292             :   std::mutex                                                         mutex_other_uav_avoidance_trajectories_;
+     293             : 
+     294             :   // subscribing to the other UAV diagnostics'
+     295             :   void callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg);
+     296             : 
+     297             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>> other_uav_diag_subscribers_;
+     298             :   std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>                  other_uav_diagnostics_;
+     299             :   std::mutex                                                              mutex_other_uav_diagnostics_;
+     300             : 
+     301             :   bool checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     302             :   bool checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     303             : 
+     304             :   mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory> ph_avoidance_trajectory_;
+     305             : 
+     306             :   ros::ServiceServer service_server_toggle_avoidance_;
+     307             :   bool               callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     308             : 
+     309             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estimation_diag_;
+     310             : 
+     311             :   // | --------------------- MPC calculation -------------------- |
+     312             : 
+     313             :   ros::Timer        timer_mpc_iteration_;
+     314             :   std::atomic<bool> mpc_synchronous_ = false;
+     315             :   ros::TimerEvent   synchronous_timer_event_;
+     316             : 
+     317             :   std::atomic<bool> mpc_timer_running_ = false;
+     318             :   void              timerMPC(const ros::TimerEvent& event);
+     319             : 
+     320             :   // | -------------------- velocity tracking ------------------- |
+     321             : 
+     322             :   ros::Timer                  timer_velocity_tracking_;
+     323             :   void                        timerVelocityTracking(const ros::TimerEvent& event);
+     324             :   ros::Time                   velocity_reference_time_;
+     325             :   mrs_msgs::VelocityReference velocity_reference_;
+     326             :   std::mutex                  mutex_velocity_reference_;
+     327             :   std::atomic<bool>           velocity_tracking_active_ = false;
+     328             : 
+     329             :   // | ------------------ avoidance trajectory ------------------ |
+     330             : 
+     331             :   ros::Timer timer_avoidance_trajectory_;
+     332             :   void       timerAvoidanceTrajectory(const ros::TimerEvent& event);
+     333             : 
+     334             :   // | ----------------------- diagnostics ---------------------- |
+     335             : 
+     336             :   ros::Timer timer_diagnostics_;
+     337             :   double     _diagnostics_rate_;
+     338             :   void       timerDiagnostics(const ros::TimerEvent& event);
+     339             : 
+     340             :   // | ------------------------ hovering ------------------------ |
+     341             : 
+     342             :   ros::Timer        timer_hover_;
+     343             :   void              timerHover(const ros::TimerEvent& event);
+     344             :   std::atomic<bool> hovering_in_progress_ = false;
+     345             :   void              toggleHover(bool in);
+     346             : 
+     347             :   // | ------------------- trajectory tracking ------------------ |
+     348             : 
+     349             :   std::tuple<bool, std::string> resumeTrajectoryTrackingImpl(void);
+     350             :   std::tuple<bool, std::string> startTrajectoryTrackingImpl(void);
+     351             :   std::tuple<bool, std::string> stopTrajectoryTrackingImpl(void);
+     352             :   std::tuple<bool, std::string> gotoTrajectoryStartImpl(void);
+     353             : 
+     354             :   // | --------------------- other routines --------------------- |
+     355             : 
+     356             :   void publishDiagnostics();
+     357             : 
+     358             :   void debugPrintState(const double throttle);
+     359             :   void debugPrintMPCResult(const double throttle);
+     360             : 
+     361             :   void setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     362             :   void setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     363             :   void setSinglePointReference(const double x, const double y, const double z, const double heading);
+     364             : 
+     365             :   std::tuple<bool, std::string, bool> loadTrajectory(const mrs_msgs::TrajectoryReference msg);
+     366             : 
+     367             :   MatrixXd                       filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed);
+     368             :   std::tuple<MatrixXd, MatrixXd> filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x, double max_speed_y);
+     369             : 
+     370             :   double checkTrajectoryForCollisions(int& first_collision_index);
+     371             : 
+     372             :   void manageConstraints(void);
+     373             :   void calculateMPC(void);
+     374             :   void iterateModel(const double& dt);
+     375             : 
+     376             :   // | ------------------------ profiler ------------------------ |
+     377             : 
+     378             :   mrs_lib::Profiler profiler;
+     379             :   bool              _profiler_enabled_ = false;
+     380             : 
+     381             :   // | ------------------------- wiggle ------------------------- |
+     382             : 
+     383             :   ros::ServiceServer service_server_wiggle_;
+     384             :   bool               callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     385             : 
+     386             :   double wiggle_phase_ = 0;
+     387             : 
+     388             :   // | --------------- dynamic reconfigure server --------------- |
+     389             : 
+     390             :   void dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, uint32_t level);
+     391             : 
+     392             :   boost::recursive_mutex                      config_mutex_;
+     393             :   typedef mrs_uav_trackers::mpc_trackerConfig Config;
+     394             :   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+     395             :   boost::shared_ptr<ReconfigureServer>        reconfigure_server_;
+     396             :   mrs_uav_trackers::mpc_trackerConfig         drs_params_;
+     397             :   std::mutex                                  mutex_drs_params_;
+     398             : };
+     399             : 
+     400             : //}
+     401             : 
+     402             : // | -------------- tracker's interface routines -------------- |
+     403             : 
+     404             : /* //{ initialize() */
+     405             : 
+     406          94 : bool MpcTracker::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     407             :                             std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     408             : 
+     409          94 :   nh_ = nh;
+     410             : 
+     411          94 :   common_handlers_  = common_handlers;
+     412          94 :   private_handlers_ = private_handlers;
+     413             : 
+     414          94 :   _uav_name_ = common_handlers->uav_name;
+     415             : 
+     416          94 :   ros::Time::waitForValid();
+     417             : 
+     418             :   // --------------------------------------------------------------
+     419             :   // |                     loading parameters                     |
+     420             :   // --------------------------------------------------------------
+     421             : 
+     422             :   // | ---------- loading params using the parent's nh ---------- |
+     423             : 
+     424         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     425             : 
+     426          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     427             : 
+     428          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     429           0 :     ROS_ERROR("[MpcTracker]: Could not load all parameters!");
+     430           0 :     return false;
+     431             :   }
+     432             : 
+     433             :   // | --------------- loading plugin's parameters -------------- |
+     434             : 
+     435          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/mpc_tracker.yaml");
+     436          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/mpc_tracker.yaml");
+     437             : 
+     438         188 :   const std::string yaml_prefix = "mrs_uav_trackers/mpc_tracker/";
+     439             : 
+     440          94 :   private_handlers->param_loader->loadParam("network/robot_names", _avoidance_other_uav_names_);
+     441             : 
+     442          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/synchronous_rate_limit", _mpc_synchronous_rate_limit_);
+     443          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/asynchronous_loop_rate", _mpc_asynchronous_rate_);
+     444             : 
+     445          94 :   if (_mpc_asynchronous_rate_ < 15) {
+     446           0 :     ROS_ERROR("[MpcTracker]: the asynchronous_loop_rate must be > 15 Hz");
+     447           0 :     return false;
+     448             :   }
+     449             : 
+     450          94 :   dt1_ = 1.0 / _mpc_asynchronous_rate_;
+     451             : 
+     452          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/enabled", drs_params_.braking_enabled);
+     453          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_braking", drs_params_.q_vel_braking);
+     454          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_no_braking", drs_params_.q_vel_no_braking);
+     455             : 
+     456          94 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/A", _mat_A_, MPC_N_STATES, MPC_N_STATES);
+     457          94 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/B", _mat_B_, MPC_N_STATES, MPC_N_INPUTS);
+     458             : 
+     459          94 :   A_ = _mat_A_;
+     460          94 :   B_ = _mat_B_;
+     461             : 
+     462          94 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/A", _mat_A_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_STATES);
+     463          94 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/B", _mat_B_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_INPUTS);
+     464             : 
+     465          94 :   A_heading_ = _mat_A_heading_;
+     466          94 :   B_heading_ = _mat_B_heading_;
+     467             : 
+     468             :   // load the MPC parameters
+     469          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/dt2", _dt2_);
+     470             : 
+     471          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_rate_);
+     472          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/position_tracking_threshold", _diag_pos_tracking_thr_);
+     473          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/orientation_tracking_threshold", _diag_heading_tracking_thr_);
+     474             : 
+     475          94 :   bool verbose_xy      = false;
+     476          94 :   bool verbose_z       = false;
+     477          94 :   bool verbose_heading = false;
+     478             : 
+     479         188 :   std::vector<double> xy_Q;
+     480         188 :   std::vector<double> z_Q;
+     481         188 :   std::vector<double> heading_Q;
+     482             : 
+     483          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/verbose", verbose_xy);
+     484          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/max_n_iterations", _max_iters_xy_);
+     485          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/Q", xy_Q);
+     486             : 
+     487          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/verbose", verbose_z);
+     488          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/max_n_iterations", _max_iters_z_);
+     489          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/Q", z_Q);
+     490             : 
+     491          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/verbose", verbose_heading);
+     492          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/max_n_iterations", _max_iters_heading_);
+     493          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/Q", heading_Q);
+     494             : 
+     495          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/enabled", drs_params_.wiggle_enabled);
+     496          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/amplitude", drs_params_.wiggle_amplitude);
+     497          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/frequency", drs_params_.wiggle_frequency);
+     498             : 
+     499          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled", collision_avoidance_enabled_);
+     500          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled_passively", collision_avoidance_enabled_passively_);
+     501          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/predicted_trajectory_publish_rate", _avoidance_trajectory_rate_);
+     502          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/correction", _avoidance_z_correction_);
+     503          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/radius", _avoidance_radius_threshold_);
+     504          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/altitude_threshold", _avoidance_z_threshold_);
+     505          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_horizontal_speed_coef", _avoidance_collision_horizontal_speed_coef_);
+     506          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_fully", _avoidance_collision_slow_down_fully_);
+     507          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_start", _avoidance_collision_slow_down_);
+     508          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_start_climbing", _avoidance_collision_start_climbing_);
+     509          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/trajectory_timeout", _collision_trajectory_timeout_);
+     510             : 
+     511          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     512           0 :     ROS_ERROR("[MpcTracker]: could not load all parameters!");
+     513           0 :     return false;
+     514             :   }
+     515             : 
+     516          94 :   ROS_INFO_STREAM("[MpcTracker]: initializing solvers with dt1 = " << dt1_);
+     517             : 
+     518          94 :   mpc_solver_y_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_y", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 1);
+     519          94 :   mpc_solver_x_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_x", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 0);
+     520          94 :   mpc_solver_z_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_z", verbose_z, _max_iters_z_, z_Q, dt1_, _dt2_, 2);
+     521             :   mpc_solver_heading_ =
+     522          94 :       std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_hdg", verbose_heading, _max_iters_heading_, heading_Q, dt1_, _dt2_, 0);
+     523             : 
+     524          94 :   mpc_x_         = MatrixXd::Zero(MPC_N_STATES, 1);
+     525          94 :   mpc_x_heading_ = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     526             : 
+     527          94 :   mpc_u_ = VectorXd::Zero(MPC_N_INPUTS);
+     528             : 
+     529          94 :   coef_time = ros::Time(0);
+     530             : 
+     531          94 :   des_x_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     532          94 :   des_y_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     533          94 :   des_z_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     534          94 :   des_z_filtered_offset_  = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     535          94 :   des_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     536             : 
+     537          94 :   service_server_wiggle_ = nh_.advertiseService("wiggle", &MpcTracker::callbackWiggle, this);
+     538             : 
+     539          94 :   pub_diagnostics_   = mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics>(nh_, "diagnostics", 1);
+     540          94 :   pub_status_string_ = mrs_lib::PublisherHandler<std_msgs::String>(nh_, "string", 1);
+     541             : 
+     542             :   // extract the numerical name
+     543          94 :   sscanf(_uav_name_.c_str(), "uav%d", &avoidance_this_uav_number_);
+     544          94 :   ROS_INFO("[MpcTracker]: Numerical ID of this UAV is %d", avoidance_this_uav_number_);
+     545          94 :   avoidance_this_uav_priority_ = avoidance_this_uav_number_;
+     546             : 
+     547             :   // exclude this drone from the list
+     548          94 :   std::vector<std::string>::iterator it = _avoidance_other_uav_names_.begin();
+     549         198 :   while (it != _avoidance_other_uav_names_.end()) {
+     550             : 
+     551         104 :     std::string temp_str = *it;
+     552             : 
+     553             :     int other_uav_priority;
+     554         104 :     sscanf(temp_str.c_str(), "uav%d", &other_uav_priority);
+     555             : 
+     556         104 :     if (other_uav_priority == avoidance_this_uav_number_) {
+     557             : 
+     558          94 :       _avoidance_other_uav_names_.erase(it);
+     559          94 :       continue;
+     560             :     }
+     561             : 
+     562          10 :     it++;
+     563             :   }
+     564             : 
+     565             :   // initialize velocity tracker
+     566             : 
+     567          94 :   velocity_reference_time_ = ros::Time(0);
+     568             : 
+     569             :   // create publishers for predicted trajectory
+     570             : 
+     571          94 :   ph_avoidance_trajectory_           = mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory>(nh_, "predicted_trajectory", 1);
+     572          94 :   ph_predicted_trajectory_debugging_ = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "predicted_trajectory_debugging", 1);
+     573          94 :   ph_mpc_reference_debugging_        = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "mpc_reference_debugging", 1, true);
+     574          94 :   ph_current_trajectory_point_       = mrs_lib::PublisherHandler<geometry_msgs::PoseStamped>(nh_, "current_trajectory_point", 1, true);
+     575             : 
+     576          94 :   pub_debug_processed_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_processed/poses", 1, true);
+     577          94 :   pub_debug_processed_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_processed/markers", 1, true);
+     578             : 
+     579             :   // preallocate predicted trajectory
+     580          94 :   predicted_trajectory_         = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     581          94 :   predicted_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     582             : 
+     583          94 :   collision_free_altitude_ = std::numeric_limits<float>::lowest();
+     584             : 
+     585             :   // collision avoidance toggle service
+     586          94 :   service_server_toggle_avoidance_ = nh_.advertiseService("collision_avoidance", &MpcTracker::callbackToggleCollisionAvoidance, this);
+     587             : 
+     588         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+     589          94 :   shopts.nh                 = nh_;
+     590          94 :   shopts.node_name          = "MpcTracker";
+     591          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     592          94 :   shopts.threadsafe         = true;
+     593          94 :   shopts.autostart          = true;
+     594          94 :   shopts.queue_size         = 10;
+     595          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     596             : 
+     597             :   // create subscribers on other drones diagnostics
+     598          94 :   if (collision_avoidance_enabled_ || collision_avoidance_enabled_passively_) {
+     599             : 
+     600         104 :     for (int i = 0; i < int(_avoidance_other_uav_names_.size()); i++) {
+     601             : 
+     602          30 :       std::string prediction_topic_name = std::string("/") + _avoidance_other_uav_names_.at(i) + "/control_manager/mpc_tracker/predicted_trajectory";
+     603          20 :       std::string diag_topic_name       = std::string("/") + _avoidance_other_uav_names_.at(i) + "/control_manager/mpc_tracker/diagnostics";
+     604             : 
+     605          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", prediction_topic_name.c_str());
+     606             : 
+     607          10 :       other_uav_trajectory_subscribers_.push_back(
+     608          20 :           mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>(shopts, prediction_topic_name, &MpcTracker::callbackOtherMavTrajectory, this));
+     609             : 
+     610          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", diag_topic_name.c_str());
+     611             : 
+     612          10 :       other_uav_diag_subscribers_.push_back(
+     613          20 :           mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>(shopts, diag_topic_name, &MpcTracker::callbackOtherMavDiagnostics, this));
+     614             :     }
+     615             :   }
+     616             : 
+     617          94 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, std::string("/") + _uav_name_ + "/estimation_manager/diagnostics");
+     618             : 
+     619             :   // | --------------- dynamic reconfigure server --------------- |
+     620             : 
+     621          94 :   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, nh_));
+     622          94 :   reconfigure_server_->updateConfig(drs_params_);
+     623          94 :   ReconfigureServer::CallbackType f = boost::bind(&MpcTracker::dynamicReconfigureCallback, this, _1, _2);
+     624          94 :   reconfigure_server_->setCallback(f);
+     625             : 
+     626             :   // | ------------------------ profiler ------------------------ |
+     627             : 
+     628          94 :   profiler = mrs_lib::Profiler(common_handlers->parent_nh, "MpcTracker", _profiler_enabled_);
+     629             : 
+     630             :   // | ------------------------- timers ------------------------- |
+     631             : 
+     632         188 :   timer_avoidance_trajectory_ = nh_.createTimer(ros::Rate(_avoidance_trajectory_rate_), &MpcTracker::timerAvoidanceTrajectory, this, false,
+     633         188 :                                                 collision_avoidance_enabled_ || collision_avoidance_enabled_passively_);
+     634          94 :   timer_diagnostics_          = nh_.createTimer(ros::Rate(_diagnostics_rate_), &MpcTracker::timerDiagnostics, this);
+     635          94 :   timer_mpc_iteration_        = nh_.createTimer(ros::Rate(_mpc_asynchronous_rate_), &MpcTracker::timerMPC, this, false, false);
+     636          94 :   timer_velocity_tracking_    = nh_.createTimer(ros::Rate(30.0), &MpcTracker::timerVelocityTracking, this, false, false);
+     637          94 :   timer_hover_                = nh_.createTimer(ros::Rate(10.0), &MpcTracker::timerHover, this, false, false);
+     638             : 
+     639             :   // | ----------------------- finish init ---------------------- |
+     640             : 
+     641          94 :   is_initialized_ = true;
+     642             : 
+     643          94 :   ROS_INFO("[MpcTracker]: initialized");
+     644             : 
+     645          94 :   return true;
+     646             : }
+     647             : 
+     648             : //}
+     649             : 
+     650             : /* //{ activate() */
+     651             : 
+     652          85 : std::tuple<bool, std::string> MpcTracker::activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     653             : 
+     654         170 :   std::stringstream ss;
+     655             : 
+     656          85 :   if (!got_constraints_) {
+     657             : 
+     658           0 :     ss << "can not activate, missing constraints";
+     659           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     660             : 
+     661           0 :     return std::tuple(false, ss.str());
+     662             :   }
+     663             : 
+     664         170 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     665             : 
+     666             :   double uav_state_heading;
+     667             : 
+     668             :   try {
+     669          85 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     670             :   }
+     671           0 :   catch (...) {
+     672           0 :     ss << "could not calculate the UAV heading";
+     673           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     674           0 :     return std::tuple(false, ss.str());
+     675             :   }
+     676             : 
+     677         170 :   MatrixXd mpc_x         = MatrixXd::Zero(MPC_N_STATES, 1);
+     678          85 :   MatrixXd mpc_x_heading = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     679             : 
+     680          85 :   if (last_tracker_cmd) {
+     681             : 
+     682             :     // set the initial condition from the last tracker's cmd
+     683             : 
+     684          83 :     if (last_tracker_cmd->use_position_horizontal) {
+     685          83 :       mpc_x(0, 0) = last_tracker_cmd->position.x;
+     686          83 :       mpc_x(4, 0) = last_tracker_cmd->position.y;
+     687             :     } else {
+     688           0 :       mpc_x(0, 0) = uav_state.pose.position.x;
+     689           0 :       mpc_x(4, 0) = uav_state.pose.position.y;
+     690             :     }
+     691             : 
+     692          83 :     if (last_tracker_cmd->use_position_vertical) {
+     693          83 :       mpc_x(8, 0) = last_tracker_cmd->position.z;
+     694             :     } else {
+     695           0 :       mpc_x(8, 0) = uav_state.pose.position.z;
+     696             :     }
+     697             : 
+     698          83 :     if (last_tracker_cmd->use_velocity_horizontal) {
+     699          83 :       mpc_x(1, 0) = last_tracker_cmd->velocity.x;
+     700          83 :       mpc_x(5, 0) = last_tracker_cmd->velocity.y;
+     701             :     } else {
+     702           0 :       mpc_x(1, 0) = uav_state.velocity.linear.x;
+     703           0 :       mpc_x(5, 0) = uav_state.velocity.linear.y;
+     704             :     }
+     705             : 
+     706          83 :     if (last_tracker_cmd->use_velocity_vertical) {
+     707          83 :       mpc_x(9, 0) = last_tracker_cmd->velocity.z;
+     708             :     } else {
+     709           0 :       mpc_x(9, 0) = uav_state.velocity.linear.z;
+     710             :     }
+     711             : 
+     712          83 :     if (last_tracker_cmd->use_acceleration) {
+     713           0 :       mpc_x(2, 0)  = last_tracker_cmd->acceleration.x;
+     714           0 :       mpc_x(6, 0)  = last_tracker_cmd->acceleration.y;
+     715           0 :       mpc_x(10, 0) = last_tracker_cmd->acceleration.z;
+     716             :     } else {
+     717          83 :       mpc_x(2, 0)  = 0;
+     718          83 :       mpc_x(6, 0)  = 0;
+     719          83 :       mpc_x(10, 0) = 0;
+     720             :     }
+     721             : 
+     722             :     // the jerks
+     723          83 :     mpc_x(3, 0)  = 0;
+     724          83 :     mpc_x(7, 0)  = 0;
+     725          83 :     mpc_x(11, 0) = 0;
+     726             : 
+     727          83 :     if (last_tracker_cmd->use_heading) {
+     728          83 :       mpc_x_heading(0, 0) = last_tracker_cmd->heading;
+     729           0 :     } else if (last_tracker_cmd->use_orientation) {
+     730             :       try {
+     731           0 :         mpc_x_heading(0, 0) = mrs_lib::AttitudeConverter(last_tracker_cmd->orientation).getHeading();
+     732             :       }
+     733           0 :       catch (...) {
+     734           0 :         mpc_x_heading(0, 0) = uav_state_heading;
+     735             :       }
+     736             :     } else {
+     737           0 :       mpc_x_heading(0, 0) = uav_state_heading;
+     738             :     }
+     739             : 
+     740          83 :     if (last_tracker_cmd->use_heading_rate) {
+     741          20 :       mpc_x_heading(1, 0) = last_tracker_cmd->heading_rate;
+     742             :     } else {
+     743          63 :       mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     744             :     }
+     745             : 
+     746          83 :     mpc_x_heading(2, 0) = 0;
+     747          83 :     mpc_x_heading(3, 0) = 0;
+     748             : 
+     749          83 :     ROS_INFO("[MpcTracker]: activated with last tracker's command");
+     750             : 
+     751             :   } else {
+     752             : 
+     753             :     // set the initial condition completely from the uav_state
+     754             : 
+     755           2 :     mpc_x(0, 0) = uav_state.pose.position.x;
+     756           2 :     mpc_x(1, 0) = uav_state.velocity.linear.x;
+     757           2 :     mpc_x(2, 0) = 0;
+     758           2 :     mpc_x(3, 0) = 0;
+     759             : 
+     760           2 :     mpc_x(4, 0) = uav_state.pose.position.y;
+     761           2 :     mpc_x(5, 0) = uav_state.velocity.linear.y;
+     762           2 :     mpc_x(6, 0) = 0;
+     763           2 :     mpc_x(7, 0) = 0;
+     764             : 
+     765           2 :     mpc_x(8, 0)  = uav_state.pose.position.z;
+     766           2 :     mpc_x(9, 0)  = uav_state.velocity.linear.z;
+     767           2 :     mpc_x(10, 0) = 0;
+     768           2 :     mpc_x(11, 0) = 0;
+     769             : 
+     770           2 :     mpc_x_heading(0, 0) = uav_state_heading;
+     771           2 :     mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     772           2 :     mpc_x_heading(2, 0) = 0;
+     773           2 :     mpc_x_heading(3, 0) = 0;
+     774             : 
+     775           2 :     ROS_INFO("[MpcTracker]: activated with uav state");
+     776             :   }
+     777             : 
+     778             :   {
+     779         170 :     std::scoped_lock lock(mutex_mpc_x_);
+     780             : 
+     781          85 :     mpc_x_         = mpc_x;
+     782          85 :     mpc_x_heading_ = mpc_x_heading;
+     783             :   }
+     784             : 
+     785          85 :   trajectory_tracking_in_progress_ = false;
+     786             : 
+     787          85 :   ss << "activated";
+     788          85 :   ROS_INFO_STREAM("[MpcTracker]: " << ss.str());
+     789             : 
+     790             :   // this is here to initialize the desired_trajectory vector
+     791             :   // if deleted (and I tried) the UAV will briefly fly to the
+     792             :   // origin after activation
+     793          85 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     794             : 
+     795          85 :   toggleHover(true);
+     796             : 
+     797          85 :   model_first_iteration_ = true;
+     798             : 
+     799          85 :   A_ = _mat_A_;
+     800          85 :   B_ = _mat_B_;
+     801             : 
+     802          85 :   A_heading_ = _mat_A_heading_;
+     803          85 :   B_heading_ = _mat_B_heading_;
+     804             : 
+     805          85 :   is_active_ = true;
+     806             : 
+     807          85 :   if (!mpc_synchronous_) {
+     808          75 :     timer_mpc_iteration_.start();
+     809             :   }
+     810             : 
+     811          85 :   return std::tuple(true, ss.str());
+     812             : }
+     813             : 
+     814             : //}
+     815             : 
+     816             : /* //{ deactivate() */
+     817             : 
+     818          40 : void MpcTracker::deactivate(void) {
+     819             : 
+     820          40 :   toggleHover(false);
+     821             : 
+     822          40 :   is_active_                       = false;
+     823          40 :   trajectory_tracking_in_progress_ = false;
+     824          40 :   model_first_iteration_           = true;
+     825             : 
+     826             :   {
+     827          40 :     std::scoped_lock lock(mutex_trajectory_tracking_states_);
+     828             : 
+     829          40 :     trajectory_current_time_ = 0;
+     830             :   }
+     831             : 
+     832          40 :   ROS_INFO("[MpcTracker]: deactivated");
+     833             : 
+     834          40 :   timer_mpc_iteration_.stop();
+     835             : 
+     836          40 :   publishDiagnostics();
+     837          40 : }
+     838             : 
+     839             : //}
+     840             : 
+     841             : /* //{ resetStatic() */
+     842             : 
+     843           0 : bool MpcTracker::resetStatic(void) {
+     844             : 
+     845           0 :   if (!is_initialized_) {
+     846           0 :     ROS_ERROR("[MpcTracker]: can not reset, not initialized");
+     847           0 :     return false;
+     848             :   }
+     849             : 
+     850           0 :   if (!is_active_) {
+     851           0 :     ROS_ERROR("[MpcTracker]: can not reset, not active");
+     852           0 :     return false;
+     853             :   }
+     854             : 
+     855           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     856             : 
+     857             :   double uav_state_heading;
+     858             : 
+     859             :   try {
+     860           0 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     861             :   }
+     862           0 :   catch (...) {
+     863           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: could not calculate the UAV heading");
+     864           0 :     return false;
+     865             :   }
+     866             : 
+     867             :   {
+     868           0 :     std::scoped_lock lock(mutex_mpc_x_);
+     869             : 
+     870             :     // set the initial condition from the odometry
+     871             : 
+     872           0 :     ROS_INFO("[MpcTracker]: reseting with uav state with no dynamics");
+     873             : 
+     874           0 :     mpc_x_(0, 0) = uav_state.pose.position.x;
+     875           0 :     mpc_x_(1, 0) = 0;
+     876           0 :     mpc_x_(2, 0) = 0;
+     877           0 :     mpc_x_(3, 0) = 0;
+     878             : 
+     879           0 :     mpc_x_(4, 0) = uav_state.pose.position.y;
+     880           0 :     mpc_x_(5, 0) = 0;
+     881           0 :     mpc_x_(6, 0) = 0;
+     882           0 :     mpc_x_(7, 0) = 0;
+     883             : 
+     884           0 :     mpc_x_(8, 0)  = uav_state.pose.position.z;
+     885           0 :     mpc_x_(9, 0)  = 0;
+     886           0 :     mpc_x_(10, 0) = 0;
+     887           0 :     mpc_x_(11, 0) = 0;
+     888             : 
+     889           0 :     mpc_x_heading_(0, 0) = uav_state_heading;
+     890           0 :     mpc_x_heading_(1, 0) = 0;
+     891           0 :     mpc_x_heading_(2, 0) = 0;
+     892           0 :     mpc_x_heading_(3, 0) = 0;
+     893             : 
+     894           0 :     trajectory_tracking_in_progress_ = false;
+     895             : 
+     896           0 :     ROS_INFO("[MpcTracker]: reseted");
+     897             :   }
+     898             : 
+     899             :   // this is here to initialize the desired_trajectory vector
+     900             :   // if deleted (and I tried) the UAV will briefly fly to the
+     901             :   // origin after activation
+     902           0 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     903             : 
+     904           0 :   return true;
+     905             : }
+     906             : 
+     907             : //}
+     908             : 
+     909             : /* //{ update() */
+     910             : 
+     911      127134 : std::optional<mrs_msgs::TrackerCommand> MpcTracker::update(const mrs_msgs::UavState&                                           uav_state,
+     912             :                                                            [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput& last_control_output) {
+     913             : 
+     914      381402 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("update");
+     915      381402 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("MpcTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     916             : 
+     917      254268 :   auto old_uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     918             : 
+     919             :   // the time from the last update call
+     920      127134 :   double dt = (uav_state.header.stamp - old_uav_state.header.stamp).toSec();
+     921             : 
+     922             :   // save the uav state
+     923      127134 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     924             : 
+     925      127134 :   if (dt > 0) {
+     926             : 
+     927      127134 :     double rate = 1.0 / dt;
+     928             : 
+     929      127134 :     update_rate_ = 0.9 * update_rate_ + 0.1 * rate;
+     930             : 
+     931      127134 :     if (mpc_synchronous_ && (update_rate_ > _mpc_synchronous_rate_limit_)) {
+     932           0 :       mpc_synchronous_ = false;
+     933           0 :       ROS_INFO("[MpcTracker]: detecting high update date (%.1f Hz > %.1f Hz), switching to asynchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     934           0 :       if (is_active_) {
+     935           0 :         timer_mpc_iteration_.start();
+     936             :       }
+     937      127134 :     } else if (!mpc_synchronous_ && (update_rate_ <= _mpc_synchronous_rate_limit_)) {
+     938          14 :       mpc_synchronous_ = true;
+     939          14 :       ROS_INFO("[MpcTracker]: detecting low update rate (%.1f Hz < %.1f Hz), switching to synchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     940          14 :       timer_mpc_iteration_.stop();
+     941             :     }
+     942             :   }
+     943             : 
+     944             :   // up to this part the update() method is evaluated even when the tracker is not active
+     945      127134 :   if (!is_active_) {
+     946       56109 :     return {};
+     947             :   }
+     948             : 
+     949      142050 :   mrs_msgs::TrackerCommand tracker_cmd;
+     950             : 
+     951       71025 :   if (!mpc_synchronous_ && (!mpc_computed_ || mpc_result_invalid_)) {
+     952             : 
+     953         128 :     ROS_WARN_THROTTLE(0.1, "[MpcTracker]: MPC not ready, returning current odom as the command");
+     954             : 
+     955             :     // set the header
+     956         128 :     tracker_cmd.header.stamp    = uav_state.header.stamp;
+     957         128 :     tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     958             : 
+     959             :     // set positions from odom
+     960         128 :     tracker_cmd.position.x              = uav_state.pose.position.x;
+     961         128 :     tracker_cmd.position.y              = uav_state.pose.position.y;
+     962         128 :     tracker_cmd.position.z              = uav_state.pose.position.z;
+     963         128 :     tracker_cmd.use_position_vertical   = 1;
+     964         128 :     tracker_cmd.use_position_horizontal = 1;
+     965             : 
+     966             :     // set velocities from odom
+     967         128 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     968         128 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     969         128 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     970         128 :     tracker_cmd.use_velocity_vertical   = 1;
+     971         128 :     tracker_cmd.use_velocity_horizontal = 1;
+     972             : 
+     973             :     // set zero accelerations
+     974         128 :     tracker_cmd.acceleration.x   = 0;
+     975         128 :     tracker_cmd.acceleration.y   = 0;
+     976         128 :     tracker_cmd.acceleration.z   = 0;
+     977         128 :     tracker_cmd.use_acceleration = 1;
+     978             : 
+     979             :     try {
+     980         128 :       tracker_cmd.heading     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     981         128 :       tracker_cmd.use_heading = 1;
+     982             :     }
+     983           0 :     catch (...) {
+     984           0 :       tracker_cmd.use_heading = 0;
+     985           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading");
+     986             :     }
+     987             : 
+     988             :     // set zero jerk
+     989         128 :     tracker_cmd.jerk.x = 0;
+     990         128 :     tracker_cmd.jerk.y = 0;
+     991         128 :     tracker_cmd.jerk.z = 0;
+     992             : 
+     993             :     try {
+     994         128 :       tracker_cmd.heading_rate     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeadingRate(uav_state.velocity.angular);
+     995         128 :       tracker_cmd.use_heading_rate = 1;
+     996             :     }
+     997           0 :     catch (...) {
+     998           0 :       tracker_cmd.use_heading_rate = 0;
+     999           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading rate");
+    1000             :     }
+    1001             : 
+    1002         128 :     return {tracker_cmd};
+    1003             :   }
+    1004             : 
+    1005       70897 :   if (mpc_synchronous_) {
+    1006             : 
+    1007        3846 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in SYNCHRONOUS mode");
+    1008             : 
+    1009        3846 :     if (synchronous_timer_event_.last_real.toSec() > 0) {
+    1010        3832 :       synchronous_timer_event_.last_real = synchronous_timer_event_.current_real;
+    1011             :     } else {
+    1012          14 :       synchronous_timer_event_.last_real = ros::Time::now();
+    1013             :     }
+    1014             : 
+    1015        3846 :     synchronous_timer_event_.current_real = ros::Time::now();
+    1016             : 
+    1017        3846 :     timerMPC(synchronous_timer_event_);
+    1018             : 
+    1019             :   } else {
+    1020       67051 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in ASYNCHRONOUS mode");
+    1021             :   }
+    1022             : 
+    1023       70897 :   if (dt > 0) {
+    1024       70897 :     iterateModel(dt);
+    1025             :   } else {
+    1026           0 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: dt !> 0, not iterating the model");
+    1027             :   }
+    1028             : 
+    1029      141794 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1030      141794 :   auto prediction_full_state  = mrs_lib::get_mutexed(mutex_prediction_full_state_, prediction_full_state_);
+    1031             : 
+    1032             :   // check whether all outputs are finite
+    1033       70897 :   bool arefinite = true;
+    1034      921661 :   for (int i = 0; i < 12; i++) {
+    1035      850764 :     if (!std::isfinite(mpc_x(i, 0))) {
+    1036           0 :       arefinite = false;
+    1037             :     }
+    1038             :   }
+    1039             : 
+    1040       70897 :   if (arefinite) {
+    1041             : 
+    1042             :     // set the desired states base on the result of the mpc
+    1043       70897 :     tracker_cmd.position.x     = mpc_x(0, 0);
+    1044       70897 :     tracker_cmd.velocity.x     = mpc_x(1, 0);
+    1045       70897 :     tracker_cmd.acceleration.x = mpc_x(2, 0);
+    1046       70897 :     tracker_cmd.jerk.x         = mpc_x(3, 0);
+    1047             : 
+    1048       70897 :     tracker_cmd.position.y     = mpc_x(4, 0);
+    1049       70897 :     tracker_cmd.velocity.y     = mpc_x(5, 0);
+    1050       70897 :     tracker_cmd.acceleration.y = mpc_x(6, 0);
+    1051       70897 :     tracker_cmd.jerk.y         = mpc_x(7, 0);
+    1052             : 
+    1053       70897 :     tracker_cmd.position.z     = mpc_x(8, 0);
+    1054       70897 :     tracker_cmd.velocity.z     = mpc_x(9, 0);
+    1055       70897 :     tracker_cmd.acceleration.z = mpc_x(10, 0);
+    1056       70897 :     tracker_cmd.jerk.z         = mpc_x(11, 0);
+    1057             : 
+    1058       70897 :     tracker_cmd.full_state_prediction = prediction_full_state;
+    1059             : 
+    1060       70897 :     tracker_cmd.use_position_vertical     = 1;
+    1061       70897 :     tracker_cmd.use_position_horizontal   = 1;
+    1062       70897 :     tracker_cmd.use_velocity_vertical     = 1;
+    1063       70897 :     tracker_cmd.use_velocity_horizontal   = 1;
+    1064       70897 :     tracker_cmd.use_acceleration          = 1;
+    1065       70897 :     tracker_cmd.use_jerk                  = 1;
+    1066       70897 :     tracker_cmd.use_full_state_prediction = 1;
+    1067             : 
+    1068             :   } else {
+    1069             : 
+    1070           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC translation outputs are not finite!");
+    1071             : 
+    1072           0 :     return {};
+    1073             :   }
+    1074             : 
+    1075       70897 :   bool heading_finite = true;
+    1076      354485 :   for (int i = 0; i < MPC_HEADING_N_STATES; i++) {
+    1077      283588 :     if (!std::isfinite(mpc_x_heading(i, 0))) {
+    1078           0 :       heading_finite = false;
+    1079             :     }
+    1080             :   }
+    1081             : 
+    1082       70897 :   if (heading_finite) {
+    1083             : 
+    1084       70897 :     tracker_cmd.heading              = mpc_x_heading(0, 0);
+    1085       70897 :     tracker_cmd.heading_rate         = mpc_x_heading(1, 0);
+    1086       70897 :     tracker_cmd.heading_acceleration = mpc_x_heading(2, 0);
+    1087       70897 :     tracker_cmd.heading_jerk         = mpc_x_heading(3, 0);
+    1088             : 
+    1089       70897 :     tracker_cmd.use_heading              = 1;
+    1090       70897 :     tracker_cmd.use_heading_rate         = 1;
+    1091       70897 :     tracker_cmd.use_heading_acceleration = 1;
+    1092       70897 :     tracker_cmd.use_heading_jerk         = 1;
+    1093             : 
+    1094             :   } else {
+    1095             : 
+    1096           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC heading output is not finite!");
+    1097             : 
+    1098           0 :     return {};
+    1099             :   }
+    1100             : 
+    1101             :   // set the header
+    1102       70897 :   tracker_cmd.header.stamp    = uav_state.header.stamp;
+    1103       70897 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+    1104             : 
+    1105             :   // u have to return a position command
+    1106             :   // can set the jerk to 0
+    1107       70897 :   return {tracker_cmd};
+    1108             : }  // namespace mpc_tracker
+    1109             : 
+    1110             : //}
+    1111             : 
+    1112             : /* //{ getStatus() */
+    1113             : 
+    1114        8213 : const mrs_msgs::TrackerStatus MpcTracker::getStatus() {
+    1115             : 
+    1116       16426 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1117        8213 :   auto trajectory_size        = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    1118             : 
+    1119             :   double des_x, des_y, des_z, des_heading;
+    1120             :   {
+    1121        8213 :     std::scoped_lock lock(mutex_des_trajectory_);
+    1122             : 
+    1123        8213 :     des_x       = des_x_trajectory_(0);
+    1124        8213 :     des_y       = des_y_trajectory_(0);
+    1125        8213 :     des_z       = des_z_trajectory_(0);
+    1126        8213 :     des_heading = des_heading_trajectory_(0);
+    1127             :   }
+    1128             : 
+    1129        8213 :   mrs_msgs::TrackerStatus tracker_status;
+    1130             : 
+    1131        8213 :   tracker_status.active            = is_active_;
+    1132        8213 :   tracker_status.callbacks_enabled = is_active_ && callbacks_enabled_ && !hovering_in_progress_;
+    1133             : 
+    1134        8213 :   tracker_status.tracking_trajectory = trajectory_tracking_in_progress_;
+    1135             : 
+    1136        8213 :   bool have_position_error   = sqrt(pow(mpc_x(0, 0) - des_x, 2) + pow(mpc_x(4, 0) - des_y, 2) + pow(mpc_x(8, 0) - des_z, 2)) > _diag_pos_tracking_thr_;
+    1137        8213 :   bool have_heading_error    = fabs(radians::diff(mpc_x_heading(0), des_heading)) > _diag_heading_tracking_thr_;
+    1138        8213 :   bool have_nonzero_velocity = fabs(mpc_x(1, 0)) > 0.1 || fabs(mpc_x(5, 0)) > 0.1 || fabs(mpc_x(9, 0)) > 0.1 || fabs(mpc_x_heading(1, 0)) > 0.1;
+    1139             : 
+    1140        8213 :   tracker_status.have_goal = trajectory_tracking_in_progress_ || hovering_in_progress_ || have_position_error || have_heading_error || have_nonzero_velocity;
+    1141             : 
+    1142        8213 :   int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    1143             : 
+    1144        8213 :   tracker_status.trajectory_length = trajectory_size;
+    1145        8213 :   tracker_status.trajectory_idx    = trajectory_tracking_idx;
+    1146             : 
+    1147        8213 :   if (trajectory_tracking_in_progress_) {
+    1148             : 
+    1149        3370 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1150             : 
+    1151        3370 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    1152             : 
+    1153        1685 :     tracker_status.trajectory_reference.header.stamp    = ros::Time::now();
+    1154        1685 :     tracker_status.trajectory_reference.header.frame_id = uav_state.header.frame_id;
+    1155             : 
+    1156        1685 :     tracker_status.trajectory_reference.reference.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1157        1685 :     tracker_status.trajectory_reference.reference.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1158        1685 :     tracker_status.trajectory_reference.reference.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1159        1685 :     tracker_status.trajectory_reference.reference.heading    = (*des_heading_whole_trajectory_)(trajectory_tracking_idx);
+    1160             : 
+    1161             :     // | ---------- publish the current trajectory point ---------- |
+    1162             : 
+    1163        3370 :     geometry_msgs::PoseStamped debug_trajectory_point;
+    1164        1685 :     debug_trajectory_point.header.stamp    = ros::Time::now();
+    1165        1685 :     debug_trajectory_point.header.frame_id = uav_state_.header.frame_id;
+    1166             : 
+    1167        1685 :     debug_trajectory_point.pose.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1168        1685 :     debug_trajectory_point.pose.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1169        1685 :     debug_trajectory_point.pose.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1170             : 
+    1171        1685 :     debug_trajectory_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(trajectory_tracking_idx));
+    1172             : 
+    1173        1685 :     ph_current_trajectory_point_.publish(debug_trajectory_point);
+    1174             :   }
+    1175             : 
+    1176       16426 :   return tracker_status;
+    1177             : }
+    1178             : 
+    1179             : //}
+    1180             : 
+    1181             : /* //{ enableCallbacks() */
+    1182             : 
+    1183        1955 : const std_srvs::SetBoolResponse::ConstPtr MpcTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+    1184             : 
+    1185        3910 :   std::stringstream ss;
+    1186             : 
+    1187        1955 :   if (cmd->data != callbacks_enabled_) {
+    1188             : 
+    1189        1678 :     callbacks_enabled_ = cmd->data;
+    1190        1678 :     ss << "callbacks %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1191             : 
+    1192             :   } else {
+    1193             : 
+    1194         277 :     ss << "callbacks were already %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1195             :   }
+    1196             : 
+    1197        3910 :   std_srvs::SetBoolResponse res;
+    1198        1955 :   res.message = ss.str();
+    1199        1955 :   res.success = true;
+    1200             : 
+    1201        3910 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+    1202             : }
+    1203             : 
+    1204             : //}
+    1205             : 
+    1206             : /* switchOdometrySource() //{ */
+    1207             : 
+    1208           5 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::switchOdometrySource(const mrs_msgs::UavState& new_uav_state) {
+    1209             : 
+    1210           5 :   odometry_reset_in_progress_ = true;
+    1211           5 :   mpc_result_invalid_         = true;
+    1212             : 
+    1213          10 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1214          10 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1215             : 
+    1216           5 :   ROS_INFO(
+    1217             :       "[MpcTracker]: start of odometry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1218             :       "%.2f], "
+    1219             :       "new odom [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: %.2f]",
+    1220             :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0), new_uav_state.pose.position.x, new_uav_state.pose.position.y,
+    1221             :       new_uav_state.pose.position.z, new_uav_state.velocity.linear.x, new_uav_state.velocity.linear.y, new_uav_state.velocity.linear.z,
+    1222             :       new_uav_state.acceleration.linear.x, new_uav_state.acceleration.linear.y, new_uav_state.acceleration.linear.z);
+    1223             : 
+    1224           5 :   timer_mpc_iteration_.stop();
+    1225           5 :   ROS_INFO("[MpcTracker]: mpc timer stopped");
+    1226             : 
+    1227           5 :   while (mpc_timer_running_) {
+    1228             : 
+    1229           2 :     ROS_DEBUG("[MpcTracker]: the mpc is in the middle of an iteration, waiting for it to finish");
+    1230           2 :     ros::Duration wait(0.001);
+    1231           2 :     wait.sleep();
+    1232             : 
+    1233           2 :     if (!mpc_timer_running_) {
+    1234           2 :       ROS_DEBUG("[MpcTracker]: mpc timer finished");
+    1235           2 :       break;
+    1236             :     }
+    1237             :   }
+    1238             : 
+    1239             :   // | --------- recalculate the goal to new coordinates -------- |
+    1240             : 
+    1241           5 :   double old_heading  = 0;
+    1242           5 :   double new_heading  = 0;
+    1243           5 :   bool   got_headings = true;
+    1244             : 
+    1245             :   try {
+    1246           5 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1247             :   }
+    1248           0 :   catch (...) {
+    1249           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+    1250           0 :     got_headings = false;
+    1251             :   }
+    1252             : 
+    1253             :   try {
+    1254           5 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+    1255             :   }
+    1256           0 :   catch (...) {
+    1257           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+    1258           0 :     got_headings = false;
+    1259             :   }
+    1260             : 
+    1261          10 :   std_srvs::TriggerResponse res;
+    1262             : 
+    1263           5 :   if (!got_headings) {
+    1264           0 :     res.message = "could not calculate the heading difference";
+    1265           0 :     res.success = false;
+    1266             : 
+    1267           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1268             :   }
+    1269             : 
+    1270             :   // calculate the difference of position
+    1271           5 :   double dx       = new_uav_state.pose.position.x - uav_state_.pose.position.x;
+    1272           5 :   double dy       = new_uav_state.pose.position.y - uav_state_.pose.position.y;
+    1273           5 :   double dz       = new_uav_state.pose.position.z - uav_state_.pose.position.z;
+    1274           5 :   double dheading = new_heading - old_heading;
+    1275             : 
+    1276           5 :   ROS_INFO("[MpcTracker]: dx %f dy %f dz %f dheading %f", dx, dy, dz, dheading);
+    1277             : 
+    1278             :   {
+    1279           5 :     std::scoped_lock lock(mutex_mpc_x_, mutex_des_trajectory_, mutex_des_whole_trajectory_, mutex_uav_state_);
+    1280             : 
+    1281           5 :     if (trajectory_set_) {
+    1282             : 
+    1283           0 :       for (int i = 0; i < trajectory_size_ + MPC_HORIZON_LENGTH; i++) {
+    1284             : 
+    1285           0 :         Eigen::Vector2d temp_vec((*des_x_whole_trajectory_)(i)-uav_state_.pose.position.x, (*des_y_whole_trajectory_)(i)-uav_state_.pose.position.y);
+    1286           0 :         temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1287             : 
+    1288           0 :         (*des_x_whole_trajectory_)(i) = new_uav_state.pose.position.x + temp_vec(0);
+    1289           0 :         (*des_y_whole_trajectory_)(i) = new_uav_state.pose.position.y + temp_vec(1);
+    1290           0 :         (*des_z_whole_trajectory_)(i) += dz;
+    1291           0 :         (*des_heading_whole_trajectory_)(i) += dheading;
+    1292             :       }
+    1293             :     }
+    1294             : 
+    1295         205 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1296             : 
+    1297         200 :       Eigen::Vector2d temp_vec(des_x_trajectory_(i) - uav_state_.pose.position.x, des_y_trajectory_(i) - uav_state_.pose.position.y);
+    1298         200 :       temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1299             : 
+    1300         200 :       des_x_trajectory_(i, 0) = new_uav_state.pose.position.x + temp_vec(0);
+    1301         200 :       des_y_trajectory_(i, 0) = new_uav_state.pose.position.y + temp_vec(1);
+    1302         200 :       des_z_trajectory_(i, 0) += dz;
+    1303         200 :       des_heading_trajectory_(i, 0) += dheading;
+    1304             :     }
+    1305             : 
+    1306             :     // update the position
+    1307             :     {
+    1308           5 :       Eigen::Vector2d temp_vec(mpc_x_(0, 0) - uav_state_.pose.position.x, mpc_x_(4, 0) - uav_state_.pose.position.y);
+    1309           5 :       temp_vec     = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1310           5 :       mpc_x_(0, 0) = new_uav_state.pose.position.x + temp_vec(0);
+    1311           5 :       mpc_x_(4, 0) = new_uav_state.pose.position.y + temp_vec(1);
+    1312           5 :       mpc_x_(8, 0) += dz;
+    1313             :     }
+    1314             : 
+    1315             :     // update the velocity
+    1316             :     {
+    1317           5 :       mpc_x_(1, 0) = new_uav_state.velocity.linear.x;
+    1318           5 :       mpc_x_(5, 0) = new_uav_state.velocity.linear.y;
+    1319             :       // we leave the z velocity as it was in the original frame
+    1320             :     }
+    1321             : 
+    1322             :     // update the acceleration
+    1323             :     {
+    1324           5 :       mpc_x_(2, 0)  = 0;
+    1325           5 :       mpc_x_(6, 0)  = 0;
+    1326           5 :       mpc_x_(10, 0) = 0;
+    1327             :     }
+    1328             : 
+    1329             :     // update the heading and its derivative
+    1330           5 :     mpc_x_heading_(0, 0) += dheading;
+    1331           5 :     mpc_x_heading_(1, 0) = new_uav_state.velocity.angular.x;
+    1332             :   }
+    1333             : 
+    1334           5 :   ROS_INFO(
+    1335             :       "[MpcTracker]: start of odometry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1336             :       "%.2f]",
+    1337             :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0));
+    1338             : 
+    1339           5 :   ROS_INFO("[MpcTracker]: starting the MPC timer");
+    1340             : 
+    1341           5 :   if (!mpc_synchronous_) {
+    1342           5 :     timer_mpc_iteration_.start();
+    1343             :   }
+    1344             : 
+    1345           5 :   odometry_reset_in_progress_ = false;
+    1346             : 
+    1347           5 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1348             : }
+    1349             : 
+    1350             : //}
+    1351             : 
+    1352             : /* //{ hover() */
+    1353             : 
+    1354           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1355             : 
+    1356           0 :   toggleHover(true);
+    1357             : 
+    1358           0 :   std::stringstream ss;
+    1359           0 :   ss << "initiating hover";
+    1360             : 
+    1361           0 :   std_srvs::TriggerResponse res;
+    1362           0 :   res.success = true;
+    1363           0 :   res.message = ss.str();
+    1364             : 
+    1365           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1366             : }
+    1367             : 
+    1368             : //}
+    1369             : 
+    1370             : /* //{ startTrajectoryTracking() */
+    1371             : 
+    1372           2 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1373           4 :   std::stringstream ss;
+    1374             : 
+    1375           4 :   auto [success, message] = startTrajectoryTrackingImpl();
+    1376             : 
+    1377           4 :   std_srvs::TriggerResponse res;
+    1378           2 :   res.success = success;
+    1379           2 :   res.message = message;
+    1380             : 
+    1381           4 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1382             : }
+    1383             : 
+    1384             : //}
+    1385             : 
+    1386             : /* //{ stopTrajectoryTracking() */
+    1387             : 
+    1388           1 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1389             : 
+    1390           2 :   auto [success, message] = stopTrajectoryTrackingImpl();
+    1391             : 
+    1392           2 :   std_srvs::TriggerResponse res;
+    1393           1 :   res.success = success;
+    1394           1 :   res.message = message;
+    1395             : 
+    1396           2 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1397             : }
+    1398             : 
+    1399             : //}
+    1400             : 
+    1401             : /* //{ resumeTrajectoryTracking() */
+    1402             : 
+    1403           1 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1404             : 
+    1405           2 :   auto [success, message] = resumeTrajectoryTrackingImpl();
+    1406             : 
+    1407           2 :   std_srvs::TriggerResponse res;
+    1408           1 :   res.success = success;
+    1409           1 :   res.message = message;
+    1410             : 
+    1411           2 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1412             : }
+    1413             : 
+    1414             : //}
+    1415             : 
+    1416             : /* //{ gotoTrajectoryStart() */
+    1417             : 
+    1418           2 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1419             : 
+    1420           4 :   auto [success, message] = gotoTrajectoryStartImpl();
+    1421             : 
+    1422           4 :   std_srvs::TriggerResponse res;
+    1423           2 :   res.success = success;
+    1424           2 :   res.message = message;
+    1425             : 
+    1426           4 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1427             : }
+    1428             : 
+    1429             : //}
+    1430             : 
+    1431             : /* //{ setConstraints() */
+    1432             : 
+    1433         325 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+    1434             : 
+    1435         325 :   if (!is_initialized_) {
+    1436           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+    1437             :   }
+    1438             : 
+    1439         325 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+    1440             : 
+    1441             :   // directly updated the speeds in the constraints
+    1442             :   // the reset needs to wait for manageConstraints()
+    1443             :   {
+    1444         650 :     std::scoped_lock lock(mutex_constraints_filtered_);
+    1445             : 
+    1446             :     // important! this needs to be done to initialize the full struct
+    1447         325 :     if (!got_constraints_) {
+    1448             : 
+    1449          94 :       constraints_filtered_ = constraints->constraints;
+    1450             : 
+    1451             :     } else {
+    1452             : 
+    1453         231 :       constraints_filtered_.horizontal_speed          = constraints->constraints.horizontal_speed;
+    1454         231 :       constraints_filtered_.vertical_ascending_speed  = constraints->constraints.vertical_ascending_speed;
+    1455         231 :       constraints_filtered_.vertical_descending_speed = constraints->constraints.vertical_descending_speed;
+    1456         231 :       constraints_filtered_.heading_speed             = constraints->constraints.heading_speed;
+    1457             :     }
+    1458             :   }
+    1459             : 
+    1460         325 :   got_constraints_ = true;
+    1461             : 
+    1462         325 :   all_constraints_set_ = false;
+    1463             : 
+    1464         325 :   ROS_INFO("[MpcTracker]: updating constraints");
+    1465             : 
+    1466         650 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+    1467         325 :   res.success = true;
+    1468         325 :   res.message = "constraints updated";
+    1469             : 
+    1470         325 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+    1471             : }
+    1472             : 
+    1473             : //}
+    1474             : 
+    1475             : /* //{ setReference() */
+    1476             : 
+    1477         247 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MpcTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+    1478             : 
+    1479         247 :   toggleHover(false);
+    1480             : 
+    1481         247 :   setGoal(cmd->reference.position.x, cmd->reference.position.y, cmd->reference.position.z, cmd->reference.heading, true);
+    1482             : 
+    1483         494 :   mrs_msgs::ReferenceSrvResponse res;
+    1484         247 :   res.success = true;
+    1485         247 :   res.message = "reference set";
+    1486             : 
+    1487         494 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+    1488             : }
+    1489             : 
+    1490             : //}
+    1491             : 
+    1492             : /* //{ setVelocityReference() */
+    1493             : 
+    1494         720 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MpcTracker::setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+    1495             : 
+    1496         720 :   if (!is_initialized_) {
+    1497           0 :     return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+    1498             :   }
+    1499             : 
+    1500             :   {
+    1501         720 :     std::scoped_lock lock(mutex_velocity_reference_);
+    1502             : 
+    1503         720 :     velocity_reference_time_ = ros::Time::now();
+    1504             : 
+    1505         720 :     velocity_reference_ = cmd->reference;
+    1506             :   }
+    1507             : 
+    1508         720 :   if (!velocity_tracking_active_) {
+    1509             : 
+    1510           4 :     ROS_INFO("[MpcTracker]: starting velocity tracking timer");
+    1511             : 
+    1512           4 :     timer_velocity_tracking_.stop();
+    1513           4 :     timer_velocity_tracking_.start();
+    1514             : 
+    1515           4 :     velocity_tracking_active_ = true;
+    1516             :   }
+    1517             : 
+    1518        1440 :   mrs_msgs::VelocityReferenceSrvResponse response;
+    1519         720 :   response.success = true;
+    1520         720 :   response.message = "reference set";
+    1521             : 
+    1522         720 :   return mrs_msgs::VelocityReferenceSrvResponse::ConstPtr(new mrs_msgs::VelocityReferenceSrvResponse(response));
+    1523             : }
+    1524             : 
+    1525             : //}
+    1526             : 
+    1527             : /* //{ setTrajectoryReference() */
+    1528             : 
+    1529           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MpcTracker::setTrajectoryReference([
+    1530             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+    1531             : 
+    1532          12 :   std::stringstream ss;
+    1533             : 
+    1534          18 :   auto [success, message, modified] = loadTrajectory(cmd->trajectory);
+    1535             : 
+    1536          12 :   mrs_msgs::TrajectoryReferenceSrvResponse response;
+    1537           6 :   response.success  = success;
+    1538           6 :   response.message  = message;
+    1539           6 :   response.modified = modified;
+    1540             : 
+    1541          12 :   return mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr(new mrs_msgs::TrajectoryReferenceSrvResponse(response));
+    1542             : }
+    1543             : 
+    1544             : //}
+    1545             : 
+    1546             : // | ------------------------ callbacks ----------------------- |
+    1547             : 
+    1548             : /* //{ callbackOtherMavTrajectory() */
+    1549             : 
+    1550         331 : void MpcTracker::callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg) {
+    1551             : 
+    1552         331 :   if (!is_initialized_) {
+    1553           0 :     return;
+    1554             :   }
+    1555             : 
+    1556         662 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavTrajectory");
+    1557             :   mrs_lib::ScopeTimer timer =
+    1558         662 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1559             : 
+    1560         331 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1561             : 
+    1562         331 :   mrs_msgs::FutureTrajectory trajectory = *msg;
+    1563             : 
+    1564             :   // the times might not be synchronized, so just remember the time of receiving it
+    1565         331 :   trajectory.stamp = ros::Time::now();
+    1566             : 
+    1567             :   // transform it from the utm origin to the currently used frame
+    1568         662 :   auto res = common_handlers_->transformer->getTransform("utm_origin", uav_state.header.frame_id, ros::Time::now());
+    1569             : 
+    1570         331 :   if (!res) {
+    1571             : 
+    1572           0 :     std::string message = "[MpcTracker]: can not transform other drone trajectory to the current frame";
+    1573           0 :     ROS_WARN_STREAM_ONCE(message);
+    1574           0 :     ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1575             : 
+    1576           0 :     return;
+    1577             :   }
+    1578             : 
+    1579         331 :   geometry_msgs::TransformStamped tf = res.value();
+    1580             : 
+    1581       13571 :   for (int i = 0; i < int(trajectory.points.size()); i++) {
+    1582             : 
+    1583       13240 :     geometry_msgs::PoseStamped original_pose;
+    1584             : 
+    1585       13240 :     original_pose.pose.position.x = trajectory.points.at(i).x;
+    1586       13240 :     original_pose.pose.position.y = trajectory.points.at(i).y;
+    1587       13240 :     original_pose.pose.position.z = trajectory.points.at(i).z;
+    1588             : 
+    1589       13240 :     original_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1590             : 
+    1591       13240 :     auto res = common_handlers_->transformer->transform(original_pose, tf);
+    1592             : 
+    1593       13240 :     if (res) {
+    1594       13240 :       trajectory.points.at(i).x = res.value().pose.position.x;
+    1595       13240 :       trajectory.points.at(i).y = res.value().pose.position.y;
+    1596       13240 :       trajectory.points.at(i).z = res.value().pose.position.z;
+    1597             :     } else {
+    1598             : 
+    1599           0 :       std::string message = "[MpcTracker]: could not transform point of other uav future trajectory!";
+    1600           0 :       ROS_WARN_STREAM_ONCE(message);
+    1601           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1602             : 
+    1603           0 :       return;
+    1604             :     }
+    1605             :   }
+    1606             : 
+    1607             :   {
+    1608         662 :     std::scoped_lock lock(mutex_other_uav_avoidance_trajectories_);
+    1609             : 
+    1610             :     // update the diagnostics
+    1611         331 :     other_uav_avoidance_trajectories_[trajectory.uav_name] = trajectory;
+    1612             :   }
+    1613             : }
+    1614             : 
+    1615             : //}
+    1616             : 
+    1617             : /* //{ callbackOtherMavDiagnostics() */
+    1618             : 
+    1619        2138 : void MpcTracker::callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg) {
+    1620             : 
+    1621        6414 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavDiagnostics");
+    1622             :   mrs_lib::ScopeTimer timer =
+    1623        6414 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1624             : 
+    1625        4276 :   mrs_msgs::MpcTrackerDiagnostics diagnostics = *msg;
+    1626             : 
+    1627             :   // fill in the current time
+    1628             :   // the other uav's time might not be synchronized with ours
+    1629        2138 :   diagnostics.header.stamp = ros::Time::now();
+    1630             : 
+    1631             :   {
+    1632        4276 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    1633             : 
+    1634        2138 :     other_uav_diagnostics_[diagnostics.uav_name] = diagnostics;
+    1635             :   }
+    1636        2138 : }
+    1637             : 
+    1638             : //}
+    1639             : 
+    1640             : /* //{ callbackToggleCollisionAvoidance() */
+    1641             : 
+    1642           0 : bool MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1643             : 
+    1644           0 :   collision_avoidance_enabled_ = req.data;
+    1645             : 
+    1646           0 :   ROS_INFO("[MpcTracker]: Collision avoidance was switched %s", collision_avoidance_enabled_ ? "TRUE" : "FALSE");
+    1647             : 
+    1648           0 :   res.message = "Collision avoidance set.";
+    1649           0 :   res.success = true;
+    1650             : 
+    1651           0 :   return true;
+    1652             : }
+    1653             : 
+    1654             : //}
+    1655             : 
+    1656             : /* callbackWiggle() //{ */
+    1657             : 
+    1658           0 : bool MpcTracker::callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1659             : 
+    1660           0 :   if (!is_initialized_) {
+    1661             : 
+    1662           0 :     res.success = false;
+    1663           0 :     res.message = "tracker not active";
+    1664           0 :     return true;
+    1665             :   }
+    1666             : 
+    1667             :   {
+    1668           0 :     std::scoped_lock lock(mutex_drs_params_);
+    1669             : 
+    1670           0 :     drs_params_.wiggle_enabled = req.data;
+    1671             : 
+    1672           0 :     reconfigure_server_->updateConfig(drs_params_);
+    1673             :   }
+    1674             : 
+    1675           0 :   res.success = true;
+    1676           0 :   res.message = "wiggle updated";
+    1677             : 
+    1678           0 :   return true;
+    1679             : }
+    1680             : 
+    1681             : //}
+    1682             : 
+    1683             : /* //{ dynamicReconfigureCallback() */
+    1684             : 
+    1685          94 : void MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, [[maybe_unused]] uint32_t level) {
+    1686             : 
+    1687         188 :   std::scoped_lock lock(mutex_drs_params_);
+    1688             : 
+    1689          94 :   drs_params_ = config;
+    1690             : 
+    1691          94 :   ROS_INFO("[MpcTracker]: DRS updated");
+    1692          94 : }
+    1693             : 
+    1694             : //}
+    1695             : 
+    1696             : // --------------------------------------------------------------
+    1697             : // |                          routines                          |
+    1698             : // --------------------------------------------------------------
+    1699             : 
+    1700             : // | --------------- mutual collision avoidance --------------- |
+    1701             : 
+    1702             : /* //{ checkCollision() */
+    1703             : 
+    1704      187240 : bool MpcTracker::checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1705             : 
+    1706      187240 :   if (mrs_lib::geometry::dist(vec2_t(ax, ay), vec2_t(bx, by)) < _avoidance_radius_threshold_ && fabs(az - bz) < _avoidance_z_threshold_) {
+    1707             : 
+    1708       12258 :     return true;
+    1709             : 
+    1710             :   } else {
+    1711             : 
+    1712      174982 :     return false;
+    1713             :   }
+    1714             : }
+    1715             : 
+    1716             : //}
+    1717             : 
+    1718             : /* //{ checkCollisionInflated() */
+    1719             : 
+    1720      187240 : bool MpcTracker::checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1721             : 
+    1722      187240 :   if (mrs_lib::geometry::dist(vec2_t(ax, ay), vec2_t(bx, by)) < _avoidance_radius_threshold_ + 1.0 && fabs(az - bz) < _avoidance_z_threshold_ + 1.0) {
+    1723             : 
+    1724       67846 :     return true;
+    1725             : 
+    1726             :   } else {
+    1727             : 
+    1728      119394 :     return false;
+    1729             :   }
+    1730             : }
+    1731             : 
+    1732             : //}
+    1733             : 
+    1734             : /* //{ checkTrajectoryForCollisions() */
+    1735             : 
+    1736             : // Check for potential collisions and return the needed altitude offset to avoid other drones
+    1737       76093 : double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
+    1738             : 
+    1739       76093 :   std::scoped_lock lock(mutex_predicted_trajectory_, mutex_des_trajectory_, mutex_other_uav_avoidance_trajectories_);
+    1740             : 
+    1741       76093 :   first_collision_index = INT_MAX;
+    1742       76093 :   avoiding_collision_   = false;
+    1743             : 
+    1744       76093 :   std::map<std::string, mrs_msgs::FutureTrajectory>::iterator u = other_uav_avoidance_trajectories_.begin();
+    1745             : 
+    1746       80774 :   while (u != other_uav_avoidance_trajectories_.end()) {
+    1747             : 
+    1748             :     // is the other's trajectory fresh enought?
+    1749        4681 :     if ((ros::Time::now() - u->second.stamp).toSec() < _collision_trajectory_timeout_) {
+    1750             : 
+    1751      191921 :       for (int v = 0; v < MPC_HORIZON_LENGTH; v++) {
+    1752             : 
+    1753             :         // check all points of the trajectory for possible collisions
+    1754      187240 :         if (checkCollision(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1755      187240 :                            predicted_trajectory_(v * MPC_N_STATES + 8, 0), u->second.points.at(v).x, u->second.points.at(v).y, u->second.points.at(v).z)) {
+    1756             : 
+    1757             :           // collision is detected
+    1758       12258 :           int other_uav_priority = INT_MAX;
+    1759             :           // get the priority of the other uav
+    1760             :           /* sscanf(u->first.c_str(), "uav%d", &other_uav_priority); */
+    1761       12258 :           other_uav_priority = u->second.priority;
+    1762             : 
+    1763             :           // check if we should be avoiding (out priority is higher, or the other uav has collision avoidance turned off)
+    1764       12258 :           if ((u->second.collision_avoidance == false) || (other_uav_priority < avoidance_this_uav_priority_)) {
+    1765             : 
+    1766             :             // we should be avoiding
+    1767        6416 :             avoiding_collision_      = true;
+    1768        6416 :             double tmp_safe_altitude = u->second.points.at(v).z + _avoidance_z_correction_;
+    1769             : 
+    1770        6416 :             if (tmp_safe_altitude > collision_free_altitude_ && v <= _avoidance_collision_start_climbing_) {
+    1771         216 :               collision_free_altitude_ = tmp_safe_altitude;
+    1772             :             }
+    1773             : 
+    1774        6416 :             ROS_ERROR_STREAM_THROTTLE(1, "[MpcTracker]: avoiding collision with uav" << other_uav_priority);
+    1775             : 
+    1776             :           } else {
+    1777             :             // the other uav should avoid us
+    1778        5842 :             ROS_WARN_STREAM_THROTTLE(1, "[MpcTracker]: detected collision with uav" << other_uav_priority << ", not avoiding (my priority is higher)");
+    1779             :           }
+    1780             :         }
+    1781             : 
+    1782      187240 :         if (checkCollisionInflated(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1783      187240 :                                    predicted_trajectory_(v * MPC_N_STATES + 8, 0), u->second.points.at(v).x, u->second.points.at(v).y,
+    1784      187240 :                                    u->second.points.at(v).z)) {
+    1785             : 
+    1786             :           // collision is detected
+    1787       67846 :           if (first_collision_index > v) {
+    1788        2578 :             first_collision_index = v;
+    1789             :           }
+    1790             :         }
+    1791             :       }
+    1792             :     }
+    1793             : 
+    1794        4681 :     u++;
+    1795             :   }
+    1796             : 
+    1797       76093 :   if (!avoiding_collision_) {
+    1798             : 
+    1799       75429 :     auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1800             : 
+    1801             :     // we are not avoiding any collisions, so we slowly reduce the collision avoidance offset to return to normal flight
+    1802       75429 :     collision_free_altitude_ -= 2.0 / (1.0 / dt1);
+    1803             : 
+    1804       75429 :     double safety_area_min_z = std::numeric_limits<double>::lowest();
+    1805             : 
+    1806       75429 :     if (collision_free_altitude_ < safety_area_min_z) {
+    1807             : 
+    1808           0 :       collision_free_altitude_ = safety_area_min_z;
+    1809             :     }
+    1810             :   }
+    1811             : 
+    1812      152186 :   return collision_free_altitude_;
+    1813             : }
+    1814             : 
+    1815             : //}
+    1816             : 
+    1817             : // | ------------------ trajectory filtering ------------------ |
+    1818             : 
+    1819             : /* //{ filterReferenceXY() */
+    1820             : 
+    1821       77111 : std::tuple<MatrixXd, MatrixXd> MpcTracker::filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x,
+    1822             :                                                              double max_speed_y) {
+    1823             : 
+    1824       77111 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1825             : 
+    1826      154222 :   auto mpc_x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1827       77111 :   auto trajectory_dt = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_dt_);
+    1828             : 
+    1829      154222 :   MatrixXd filtered_x_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1830      154222 :   MatrixXd filtered_y_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1831             : 
+    1832             :   double difference_x;
+    1833             :   double difference_y;
+    1834             :   double max_sample_x;
+    1835             :   double max_sample_y;
+    1836             : 
+    1837     3161550 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1838             : 
+    1839     3084440 :     if (i == 0) {
+    1840       77111 :       max_sample_x = max_speed_x * dt1;
+    1841       77111 :       max_sample_y = max_speed_y * dt1;
+    1842       77111 :       difference_x = des_x_trajectory(i, 0) - mpc_x(0, 0);
+    1843       77111 :       difference_y = des_y_trajectory(i, 0) - mpc_x(4, 0);
+    1844             :     } else {
+    1845     3007329 :       max_sample_x = max_speed_x * _dt2_;
+    1846     3007329 :       max_sample_y = max_speed_y * _dt2_;
+    1847     3007329 :       difference_x = des_x_trajectory(i, 0) - filtered_x_trajectory(i - 1, 0);
+    1848     3007329 :       difference_y = des_y_trajectory(i, 0) - filtered_y_trajectory(i - 1, 0);
+    1849             :     }
+    1850             : 
+    1851     3084440 :     if (!trajectory_tracking_in_progress_) {
+    1852             : 
+    1853     2469680 :       double direction_angle  = atan2(difference_y, difference_x);
+    1854     2469680 :       double max_dir_sample_x = abs(max_sample_x * cos(direction_angle));
+    1855     2469680 :       double max_dir_sample_y = abs(max_sample_y * sin(direction_angle));
+    1856             : 
+    1857     2469680 :       if (max_sample_x > max_dir_sample_x) {
+    1858      287277 :         max_sample_x = max_dir_sample_x;
+    1859             :       }
+    1860     2469680 :       if (max_sample_y > max_dir_sample_y) {
+    1861     2469101 :         max_sample_y = max_dir_sample_y;
+    1862             :       }
+    1863             : 
+    1864             :       // saturate the difference
+    1865     2469680 :       if (difference_x > max_sample_x)
+    1866      151128 :         difference_x = max_sample_x;
+    1867     2318552 :       else if (difference_x < -max_sample_x)
+    1868      181318 :         difference_x = -max_sample_x;
+    1869             : 
+    1870     2469680 :       if (difference_y > max_sample_y)
+    1871       82171 :         difference_y = max_sample_y;
+    1872     2387509 :       else if (difference_y < -max_sample_y)
+    1873      196439 :         difference_y = -max_sample_y;
+    1874             :     }
+    1875             : 
+    1876     3084440 :     if (i == 0) {
+    1877       77111 :       filtered_x_trajectory(i, 0) = mpc_x(0, 0) + difference_x;
+    1878       77111 :       filtered_y_trajectory(i, 0) = mpc_x(4, 0) + difference_y;
+    1879             :     } else {
+    1880     3007329 :       filtered_x_trajectory(i, 0) = filtered_x_trajectory(i - 1, 0) + difference_x;
+    1881     3007329 :       filtered_y_trajectory(i, 0) = filtered_y_trajectory(i - 1, 0) + difference_y;
+    1882             :     }
+    1883             :   }
+    1884             : 
+    1885             :   // | ----------------------- add wiggle ----------------------- |
+    1886             : 
+    1887       77111 :   auto [wiggle_enabled, wiggle_amplitude, wiggle_frequency_] =
+    1888       77111 :       mrs_lib::get_mutexed(mutex_drs_params_, drs_params_.wiggle_enabled, drs_params_.wiggle_amplitude, drs_params_.wiggle_frequency);
+    1889             : 
+    1890       77111 :   if (wiggle_enabled) {
+    1891             : 
+    1892           0 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1893           0 :       filtered_x_trajectory(i, 0) += wiggle_amplitude * cos(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1894           0 :       filtered_y_trajectory(i, 0) += wiggle_amplitude * sin(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1895             :     }
+    1896             : 
+    1897           0 :     wiggle_phase_ += wiggle_frequency_ * dt1 * 2 * M_PI;
+    1898             : 
+    1899           0 :     if (wiggle_phase_ > M_PI) {
+    1900           0 :       wiggle_phase_ -= 2 * M_PI;
+    1901             :     }
+    1902             :   }
+    1903             : 
+    1904      154222 :   return std::make_tuple(filtered_x_trajectory, filtered_y_trajectory);
+    1905             : }
+    1906             : 
+    1907             : //}
+    1908             : 
+    1909             : /* //{ filterReferenceZ() */
+    1910             : 
+    1911       77111 : MatrixXd MpcTracker::filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed) {
+    1912             : 
+    1913      154222 :   auto mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1914             : 
+    1915       77111 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1916             : 
+    1917             :   double difference_z;
+    1918             :   double max_sample_z;
+    1919             : 
+    1920       77111 :   MatrixXd filtered_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1921             : 
+    1922       77111 :   double current_z = mpc_x(8, 0);
+    1923             : 
+    1924     3161550 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1925             : 
+    1926     3084440 :     if (i == 0) {
+    1927             : 
+    1928       77111 :       difference_z = des_z_trajectory(i, 0) - current_z;
+    1929             : 
+    1930       77111 :       if (difference_z > 0) {
+    1931       31941 :         max_sample_z = max_ascending_speed * dt1;
+    1932             :       } else {
+    1933       45170 :         max_sample_z = max_descending_speed * dt1;
+    1934             :       }
+    1935             : 
+    1936             :     } else {
+    1937             : 
+    1938     3007329 :       difference_z = des_z_trajectory(i, 0) - filtered_trajectory(i - 1, 0);
+    1939             : 
+    1940     3007329 :       if (difference_z > 0) {
+    1941      200302 :         max_sample_z = max_ascending_speed * _dt2_;
+    1942             :       } else {
+    1943     2807027 :         max_sample_z = max_descending_speed * _dt2_;
+    1944             :       }
+    1945             :     }
+    1946             : 
+    1947     3084440 :     if (!trajectory_tracking_in_progress_) {
+    1948             : 
+    1949             :       // saturate the difference
+    1950     2469800 :       if (difference_z > max_sample_z)
+    1951       80303 :         difference_z = max_sample_z;
+    1952     2389497 :       else if (difference_z < -max_sample_z)
+    1953       18962 :         difference_z = -max_sample_z;
+    1954             :     }
+    1955             : 
+    1956     3084440 :     if (i == 0) {
+    1957       77111 :       filtered_trajectory(i, 0) = current_z + difference_z;
+    1958             :     } else {
+    1959     3007329 :       filtered_trajectory(i, 0) = filtered_trajectory(i - 1, 0) + difference_z;
+    1960             :     }
+    1961             :   }
+    1962             : 
+    1963      154222 :   return filtered_trajectory;
+    1964             : }
+    1965             : 
+    1966             : //}
+    1967             : 
+    1968             : /* //{ manageConstraints() */
+    1969             : 
+    1970       77111 : void MpcTracker::manageConstraints() {
+    1971             : 
+    1972       77111 :   if (!got_constraints_) {
+    1973       76961 :     return;
+    1974             :   }
+    1975             : 
+    1976       77111 :   if (all_constraints_set_) {
+    1977       76961 :     return;
+    1978             :   }
+    1979             : 
+    1980         150 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1981         300 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1982             : 
+    1983         300 :   bool can_change = (fabs(mpc_x(1, 0)) < constraints.horizontal_speed) && (fabs(mpc_x(2, 0)) < constraints.horizontal_acceleration) &&
+    1984         150 :                     (fabs(mpc_x(3, 0)) < constraints.horizontal_jerk) && (fabs(mpc_x(5, 0)) < constraints.horizontal_speed) &&
+    1985         150 :                     (fabs(mpc_x(6, 0)) < constraints.horizontal_acceleration) && (fabs(mpc_x(7, 0)) < constraints.horizontal_jerk) &&
+    1986         150 :                     (mpc_x(9, 0) < constraints.vertical_ascending_speed) && (mpc_x(9, 0) > -constraints.vertical_descending_speed) &&
+    1987         150 :                     (mpc_x(10, 0) < constraints.vertical_ascending_acceleration) && (mpc_x(10, 0) > -constraints.vertical_descending_acceleration) &&
+    1988         150 :                     (mpc_x(11, 0) < constraints.vertical_ascending_jerk) && (mpc_x(11, 0) > -constraints.vertical_descending_jerk) &&
+    1989         450 :                     (fabs(mpc_x_heading(1, 0)) < constraints.heading_speed) && (fabs(mpc_x_heading(2, 0)) < constraints.heading_acceleration) &&
+    1990         150 :                     (fabs(mpc_x_heading(3, 0)) < constraints.heading_jerk);
+    1991             : 
+    1992         150 :   if (can_change) {
+    1993             : 
+    1994             :     {
+    1995         150 :       std::scoped_lock lock(mutex_constraints_filtered_);
+    1996             : 
+    1997         150 :       constraints_filtered_.horizontal_acceleration = constraints.horizontal_acceleration;
+    1998         150 :       constraints_filtered_.horizontal_jerk         = constraints.horizontal_jerk;
+    1999         150 :       constraints_filtered_.horizontal_snap         = constraints.horizontal_snap;
+    2000             : 
+    2001         150 :       constraints_filtered_.vertical_ascending_acceleration = constraints.vertical_ascending_acceleration;
+    2002         150 :       constraints_filtered_.vertical_ascending_jerk         = constraints.vertical_ascending_jerk;
+    2003         150 :       constraints_filtered_.vertical_ascending_snap         = constraints.vertical_ascending_snap;
+    2004             : 
+    2005         150 :       constraints_filtered_.vertical_descending_acceleration = constraints.vertical_descending_acceleration;
+    2006         150 :       constraints_filtered_.vertical_descending_jerk         = constraints.vertical_descending_jerk;
+    2007         150 :       constraints_filtered_.vertical_descending_snap         = constraints.vertical_descending_snap;
+    2008             : 
+    2009         150 :       constraints_filtered_.heading_acceleration = constraints.heading_acceleration;
+    2010         150 :       constraints_filtered_.heading_jerk         = constraints.heading_jerk;
+    2011         150 :       constraints_filtered_.heading_snap         = constraints.heading_snap;
+    2012             :     }
+    2013             : 
+    2014         150 :     ROS_INFO_THROTTLE(1.0, "[MpcTracker]: all constraints succesfully applied");
+    2015         150 :     all_constraints_set_ = true;
+    2016             : 
+    2017             :   } else {
+    2018           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: slowing down to apply new constraints");
+    2019             :   }
+    2020             : }
+    2021             : 
+    2022             : //}
+    2023             : 
+    2024             : /* //{ calculateMPC() */
+    2025             : 
+    2026       77111 : void MpcTracker::calculateMPC() {
+    2027             : 
+    2028       77111 :   std::scoped_lock lock(mutex_mpc_calculation_);
+    2029             : 
+    2030       77111 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2031             : 
+    2032       77111 :   ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: MPC calculation dt = " << dt1);
+    2033             : 
+    2034       77111 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_filtered_, constraints_filtered_);
+    2035       77111 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2036       77111 :   auto uav_state              = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2037       77111 :   auto drs_params             = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    2038             : 
+    2039       77111 :   MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    2040             :   {
+    2041      154222 :     std::scoped_lock lock(mutex_des_trajectory_);
+    2042             : 
+    2043       77111 :     des_x_trajectory       = des_x_trajectory_;
+    2044       77111 :     des_y_trajectory       = des_y_trajectory_;
+    2045       77111 :     des_z_trajectory       = des_z_trajectory_;
+    2046       77111 :     des_heading_trajectory = des_heading_trajectory_;
+    2047             :   }
+    2048             : 
+    2049       77111 :   int    first_collision_index = INT_MAX;
+    2050       77111 :   double lowest_z              = std::numeric_limits<double>::max();
+    2051             : 
+    2052       77111 :   if (collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    2053             : 
+    2054             :     // determine the lowest point in our trajectory
+    2055     3119813 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2056     3043720 :       if (des_z_trajectory_(i, 0) < lowest_z) {
+    2057      107295 :         lowest_z = des_z_trajectory_(i, 0);
+    2058             :       }
+    2059             :     }
+    2060             : 
+    2061             :     // check other drone trajectories for collisions
+    2062       76093 :     minimum_collison_free_altitude_ = checkTrajectoryForCollisions(first_collision_index);
+    2063             : 
+    2064             :   } else {
+    2065             : 
+    2066        1018 :     minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+    2067             :   }
+    2068             : 
+    2069       77111 :   double max_speed_x = constraints.horizontal_speed;
+    2070       77111 :   double max_speed_y = constraints.horizontal_speed;
+    2071       77111 :   double max_speed_z = constraints.vertical_ascending_speed;
+    2072       77111 :   double min_speed_z = constraints.vertical_descending_speed;
+    2073             : 
+    2074       77111 :   double max_acc_x = constraints.horizontal_acceleration;
+    2075       77111 :   double max_acc_y = constraints.horizontal_acceleration;
+    2076       77111 :   double max_acc_z = constraints.vertical_ascending_acceleration;
+    2077       77111 :   double min_acc_z = constraints.vertical_descending_acceleration;
+    2078             : 
+    2079       77111 :   double max_snap_x = constraints.horizontal_snap;
+    2080       77111 :   double max_snap_y = constraints.horizontal_snap;
+    2081       77111 :   double max_snap_z = constraints.vertical_ascending_snap;
+    2082       77111 :   double min_snap_z = constraints.vertical_descending_snap;
+    2083             : 
+    2084       77111 :   double max_jerk_x = constraints.horizontal_jerk;
+    2085       77111 :   double max_jerk_y = constraints.horizontal_jerk;
+    2086       77111 :   double max_jerk_z = constraints.vertical_ascending_jerk;
+    2087       77111 :   double min_jerk_z = constraints.vertical_descending_jerk;
+    2088             : 
+    2089       77111 :   collision_avoidance_affecting_me_ = false;
+    2090             : 
+    2091       77111 :   if (first_collision_index < MPC_HORIZON_LENGTH) {
+    2092             : 
+    2093        2578 :     collision_avoidance_affecting_me_ = true;
+    2094             :     // the tmp variable is used to scale the speed of our drone in collision avoidance, depending on how far away the collision is
+    2095        2578 :     double tmp = 0;
+    2096             : 
+    2097        2578 :     if (first_collision_index <= _avoidance_collision_slow_down_fully_) {
+    2098        2578 :       tmp = 1;
+    2099           0 :     } else if (first_collision_index <= _avoidance_collision_slow_down_) {
+    2100           0 :       tmp = 1.0 - ((double)(first_collision_index - _avoidance_collision_slow_down_fully_)) /
+    2101           0 :                       (double)(_avoidance_collision_slow_down_ - _avoidance_collision_slow_down_fully_);
+    2102           0 :       tmp = tmp * tmp;
+    2103             :     }
+    2104             : 
+    2105        2578 :     if (!std::isfinite(tmp)) {
+    2106           0 :       tmp = 1.0;
+    2107           0 :       ROS_ERROR("[MpcTracker]: NaN detected in variable 'tmp', setting it to 1.0 and returning!!!");
+    2108           0 :       return;
+    2109        2578 :     } else if (tmp > 1.0) {
+    2110           0 :       tmp = 1.0;
+    2111        2578 :     } else if (tmp < 0.0) {
+    2112           0 :       tmp = 0.0;
+    2113             :     }
+    2114             : 
+    2115        2578 :     if (tmp > coef_scaler) {
+    2116           6 :       coef_scaler = tmp;
+    2117           6 :       coef_time   = ros::Time::now();
+    2118             :     }
+    2119        2578 :     if ((ros::Time::now() - coef_time).toSec() > 2.0) {
+    2120        2115 :       coef_scaler = tmp;
+    2121             :     }
+    2122             : 
+    2123             :     // we are close to a possible collision, better slow down a bit to give everyone more time
+    2124        2578 :     max_speed_x = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2125        2578 :     max_speed_y = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2126             :   }
+    2127             : 
+    2128       77111 :   if (collision_free_altitude_ > lowest_z) {
+    2129             : 
+    2130        1529 :     collision_avoidance_affecting_me_ = true;
+    2131        1529 :     max_speed_x                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2132        1529 :     max_speed_y                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2133             :   }
+    2134             : 
+    2135             :   // first control input generated by MPC
+    2136      154222 :   VectorXd mpc_u         = VectorXd::Zero(MPC_N_INPUTS);
+    2137       77111 :   double   mpc_u_heading = 0;
+    2138             : 
+    2139       77111 :   double iters_z       = 0;
+    2140       77111 :   double iters_x       = 0;
+    2141       77111 :   double iters_y       = 0;
+    2142       77111 :   double iters_heading = 0;
+    2143             : 
+    2144       77111 :   ros::Time time_begin = ros::Time::now();
+    2145             : 
+    2146      154222 :   MatrixXd des_z_filtered = filterReferenceZ(des_z_trajectory, max_speed_z, min_speed_z);
+    2147             : 
+    2148     3161550 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2149     3084440 :     if (des_z_filtered(i, 0) < minimum_collison_free_altitude_) {
+    2150       60162 :       des_z_filtered_offset_(i, 0) = minimum_collison_free_altitude_;
+    2151             :     } else {
+    2152     3024278 :       des_z_filtered_offset_(i, 0) = des_z_filtered(i, 0);
+    2153             :     }
+    2154             :   }
+    2155             : 
+    2156             :   // | ----------------- prepare the references ----------------- |
+    2157             : 
+    2158             :   // | -------------------- MPC solver z-axis ------------------- |
+    2159             : 
+    2160       77111 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2161       58119 :     mpc_solver_z_->setVelQ(drs_params.q_vel_braking);
+    2162             :   } else {
+    2163       18992 :     mpc_solver_z_->setVelQ(drs_params.q_vel_no_braking);
+    2164             :   }
+    2165             : 
+    2166      154222 :   MatrixXd initial_z = MatrixXd::Zero(4, 1);
+    2167             : 
+    2168       77111 :   initial_z(0, 0) = mpc_x(8, 0);
+    2169       77111 :   initial_z(1, 0) = mpc_x(9, 0);
+    2170       77111 :   initial_z(2, 0) = mpc_x(10, 0);
+    2171       77111 :   initial_z(3, 0) = mpc_x(11, 0);
+    2172             : 
+    2173       77111 :   mpc_solver_z_->setDt(dt1);
+    2174       77111 :   mpc_solver_z_->setInitialState(initial_z);
+    2175       77111 :   mpc_solver_z_->loadReference(des_z_filtered_offset_);
+    2176       77111 :   mpc_solver_z_->setLimits(max_speed_z, min_speed_z, max_acc_z, min_acc_z, max_jerk_z, min_jerk_z, max_snap_z, min_snap_z);
+    2177       77111 :   iters_z += mpc_solver_z_->solveMPC();
+    2178             : 
+    2179             :   {
+    2180      154222 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2181             : 
+    2182       77111 :     mpc_solver_z_->getStates(predicted_trajectory_);
+    2183             :   }
+    2184             : 
+    2185       77111 :   mpc_u(2) = mpc_solver_z_->getFirstControlInput();
+    2186             : 
+    2187             :   // if we are climbing to avoid a collision, reduce or arrest our horizontal velocity
+    2188             :   double ascend;
+    2189             :   {
+    2190       77111 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2191             : 
+    2192       77111 :     ascend = (predicted_trajectory_(10, 0) / max_speed_z);
+    2193             :   }
+    2194             : 
+    2195       77111 :   if (ascend > 0 && collision_free_altitude_ > lowest_z) {
+    2196         710 :     max_speed_y = max_speed_y * (1.0 - ascend);
+    2197         710 :     max_speed_x = max_speed_x * (1.0 - ascend);
+    2198             :   }
+    2199             : 
+    2200      231333 :   auto [des_x_filtered, des_y_filtered] = filterReferenceXY(des_x_trajectory, des_y_trajectory, max_speed_x, max_speed_y);
+    2201             : 
+    2202             :   // unwrap the heading reference
+    2203             : 
+    2204       77111 :   des_heading_trajectory(0, 0) = sradians::unwrap(des_heading_trajectory(0, 0), mpc_x_heading(0));
+    2205             : 
+    2206     3084440 :   for (int i = 1; i < MPC_HORIZON_LENGTH; i++) {
+    2207     3007329 :     des_heading_trajectory(i, 0) = sradians::unwrap(des_heading_trajectory(i, 0), des_heading_trajectory(i - 1, 0));
+    2208             :   }
+    2209             : 
+    2210             :   // | -------------------- MPC solver x-axis ------------------- |
+    2211             : 
+    2212       77111 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2213       58117 :     mpc_solver_x_->setVelQ(drs_params.q_vel_braking);
+    2214             :   } else {
+    2215       18994 :     mpc_solver_x_->setVelQ(drs_params.q_vel_no_braking);
+    2216             :   }
+    2217             : 
+    2218      154222 :   MatrixXd initial_x = MatrixXd::Zero(4, 1);
+    2219             : 
+    2220       77111 :   initial_x(0, 0) = mpc_x(0, 0);
+    2221       77111 :   initial_x(1, 0) = mpc_x(1, 0);
+    2222       77111 :   initial_x(2, 0) = mpc_x(2, 0);
+    2223       77111 :   initial_x(3, 0) = mpc_x(3, 0);
+    2224             : 
+    2225       77111 :   mpc_solver_x_->setDt(dt1);
+    2226       77111 :   mpc_solver_x_->setInitialState(initial_x);
+    2227       77111 :   mpc_solver_x_->loadReference(des_x_filtered);
+    2228       77111 :   mpc_solver_x_->setLimits(max_speed_x, max_speed_x, max_acc_x, max_acc_x, max_jerk_x, max_jerk_x, max_snap_x, max_snap_x);
+    2229       77111 :   iters_x += mpc_solver_x_->solveMPC();
+    2230             : 
+    2231             :   {
+    2232      154222 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2233             : 
+    2234       77111 :     mpc_solver_x_->getStates(predicted_trajectory_);
+    2235             :   }
+    2236             : 
+    2237       77111 :   mpc_u(0) = mpc_solver_x_->getFirstControlInput();
+    2238             : 
+    2239             :   // | -------------------- MPC solver y-axis ------------------- |
+    2240             : 
+    2241       77111 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2242       58117 :     mpc_solver_y_->setVelQ(drs_params.q_vel_braking);
+    2243             :   } else {
+    2244       18994 :     mpc_solver_y_->setVelQ(drs_params.q_vel_no_braking);
+    2245             :   }
+    2246             : 
+    2247      154222 :   MatrixXd initial_y = MatrixXd::Zero(4, 1);
+    2248             : 
+    2249       77111 :   initial_y(0, 0) = mpc_x(4, 0);
+    2250       77111 :   initial_y(1, 0) = mpc_x(5, 0);
+    2251       77111 :   initial_y(2, 0) = mpc_x(6, 0);
+    2252       77111 :   initial_y(3, 0) = mpc_x(7, 0);
+    2253             : 
+    2254       77111 :   mpc_solver_y_->setDt(dt1);
+    2255       77111 :   mpc_solver_y_->setInitialState(initial_y);
+    2256       77111 :   mpc_solver_y_->loadReference(des_y_filtered);
+    2257       77111 :   mpc_solver_y_->setLimits(max_speed_y, max_speed_y, max_acc_y, max_acc_y, max_jerk_y, max_jerk_y, max_snap_y, max_snap_y);
+    2258       77111 :   iters_y += mpc_solver_y_->solveMPC();
+    2259             :   {
+    2260      154222 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2261             : 
+    2262       77111 :     mpc_solver_y_->getStates(predicted_trajectory_);
+    2263             :   }
+    2264       77111 :   mpc_u(1) = mpc_solver_y_->getFirstControlInput();
+    2265             : 
+    2266             :   // | ------------------- MPC solver heading ------------------- |
+    2267             : 
+    2268       77111 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2269       58117 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_braking);
+    2270             :   } else {
+    2271       18994 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_no_braking);
+    2272             :   }
+    2273             : 
+    2274       77111 :   mpc_solver_heading_->setDt(dt1);
+    2275       77111 :   mpc_solver_heading_->setInitialState(mpc_x_heading);
+    2276       77111 :   mpc_solver_heading_->loadReference(des_heading_trajectory);
+    2277       77111 :   mpc_solver_heading_->setLimits(constraints.heading_speed, constraints.heading_speed, constraints.heading_acceleration, constraints.heading_acceleration,
+    2278             :                                  constraints.heading_jerk, constraints.heading_jerk, constraints.heading_snap, constraints.heading_snap);
+    2279       77111 :   iters_heading += mpc_solver_heading_->solveMPC();
+    2280             :   {
+    2281      154222 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2282             : 
+    2283       77111 :     mpc_solver_heading_->getStates(predicted_heading_trajectory_);
+    2284             :   }
+    2285       77111 :   mpc_u_heading = mpc_solver_heading_->getFirstControlInput();
+    2286             : 
+    2287             :   {
+    2288       77111 :     bool saturating = false;
+    2289             : 
+    2290       77111 :     if (mpc_u(0) > max_snap_x * 1.01) {
+    2291           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2292           0 :       mpc_u(0)   = max_snap_x;
+    2293           0 :       saturating = true;
+    2294             :     }
+    2295       77111 :     if (mpc_u(0) < -max_snap_x * 1.01) {
+    2296           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2297           0 :       mpc_u(0)   = -max_snap_x;
+    2298           0 :       saturating = true;
+    2299             :     }
+    2300       77111 :     if (mpc_u(1) > max_snap_y * 1.01) {
+    2301           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2302           0 :       mpc_u(1)   = max_snap_y;
+    2303           0 :       saturating = true;
+    2304             :     }
+    2305       77111 :     if (mpc_u(1) < -max_snap_y * 1.01) {
+    2306           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2307           0 :       mpc_u(1)   = -max_snap_y;
+    2308           0 :       saturating = true;
+    2309             :     }
+    2310       77111 :     if (mpc_u(2) > max_snap_z * 1.01) {
+    2311           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2312           0 :       mpc_u(2)   = max_snap_z;
+    2313           0 :       saturating = true;
+    2314             :     }
+    2315       77111 :     if (mpc_u(2) < -min_snap_z * 1.01) {
+    2316           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2317           0 :       mpc_u(2)   = -min_snap_z;
+    2318           0 :       saturating = true;
+    2319             :     }
+    2320             : 
+    2321       77111 :     if (saturating) {
+    2322           0 :       debugPrintState(0.1);
+    2323           0 :       debugPrintMPCResult(0.1);
+    2324             :     }
+    2325             :   }
+    2326             : 
+    2327             :   {
+    2328       77111 :     std::scoped_lock lock(mutex_mpc_u_);
+    2329             : 
+    2330       77111 :     mpc_u_         = mpc_u;
+    2331       77111 :     mpc_u_heading_ = mpc_u_heading;
+    2332             :   }
+    2333             : 
+    2334       77111 :   double mpc_solver_time = (ros::Time::now() - time_begin).toSec();
+    2335       77111 :   if (mpc_solver_time > dt1 || iters_x > _max_iters_xy_ || iters_y > _max_iters_xy_ || iters_z > _max_iters_z_ || iters_heading > _max_iters_heading_) {
+    2336           0 :     ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: Total MPC solver time: " << mpc_solver_time << " iters X: " << iters_x << "/" << _max_iters_xy_
+    2337             :                                                                            << " iters Y:  " << iters_y << "/" << _max_iters_xy_ << " iters Z: " << iters_z
+    2338             :                                                                            << "/" << _max_iters_z_ << " iters heading: " << iters_heading << "/"
+    2339             :                                                                            << _max_iters_heading_);
+    2340             :   }
+    2341             : 
+    2342       77111 :   future_was_predicted_ = true;
+    2343             : 
+    2344             :   // | ------------- breaking for the next iteration ------------ |
+    2345             : 
+    2346      145333 :   if (drs_params.braking_enabled && (std::abs(des_x_filtered(MPC_HORIZON_LENGTH - 6) - des_x_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2347      132089 :       (std::abs(des_y_filtered(MPC_HORIZON_LENGTH - 6) - des_y_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2348      218089 :       (std::abs(des_z_filtered(MPC_HORIZON_LENGTH - 6) - des_z_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2349       62252 :       (std::abs(radians::diff(des_heading_trajectory(MPC_HORIZON_LENGTH - 6), des_heading_trajectory(MPC_HORIZON_LENGTH - 1))) <= 0.1)) {
+    2350             : 
+    2351       62112 :     brake_ = true;
+    2352       62112 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: braking");
+    2353             : 
+    2354             :   } else {
+    2355       14999 :     brake_ = false;
+    2356             :   }
+    2357             : 
+    2358             :   /* publish mpc reference //{ */
+    2359             : 
+    2360             :   {
+    2361      154222 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2362       77111 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2363       77111 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    2364             : 
+    2365             :     {
+    2366      154222 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    2367             : 
+    2368     3161550 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2369             : 
+    2370     3084440 :         geometry_msgs::Pose new_pose;
+    2371             : 
+    2372     3084440 :         new_pose.position.x = des_x_filtered(i, 0);
+    2373     3084440 :         new_pose.position.y = des_y_filtered(i, 0);
+    2374     3084440 :         new_pose.position.z = des_z_filtered(i, 0);
+    2375             : 
+    2376     3084440 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(i));
+    2377             : 
+    2378     3084440 :         debug_trajectory_out.poses.push_back(new_pose);
+    2379             :       }
+    2380             :     }
+    2381             : 
+    2382       77111 :     ph_mpc_reference_debugging_.publish(debug_trajectory_out);
+    2383             :   }
+    2384             : 
+    2385             :   //}
+    2386             : }
+    2387             : 
+    2388             : //}
+    2389             : 
+    2390             : /* iterateModel() //{ */
+    2391             : 
+    2392       70897 : void MpcTracker::iterateModel(const double& dt) {
+    2393             : 
+    2394       70897 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2395             : 
+    2396       70897 :   if (model_first_iteration_) {
+    2397             : 
+    2398          84 :     model_iteration_last_time_ = ros::Time::now();
+    2399          84 :     model_first_iteration_     = false;
+    2400             : 
+    2401             :   } else {
+    2402             : 
+    2403       70813 :     dt1 = 0.9 * dt1 + 0.1 * dt;
+    2404             : 
+    2405       70813 :     mrs_lib::set_mutexed(mutex_dt1_, dt1, dt1_);
+    2406       70813 :     timer_mpc_iteration_.setPeriod(ros::Duration(dt1), false);
+    2407             : 
+    2408             :     // clang-format off
+    2409       70813 :     A_ << 1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,           0, 0,   0,           0,
+    2410       70813 :           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,           0, 0,   0,           0,
+    2411       70813 :           0, 0,   1,           dt1,         0, 0,   0,           0,           0, 0,   0,           0,
+    2412       70813 :           0, 0,   0,           1,           0, 0,   0,           0,           0, 0,   0,           0,
+    2413       70813 :           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,
+    2414       70813 :           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,
+    2415       70813 :           0, 0,   0,           0,           0, 0,   1,           dt1,         0, 0,   0,           0,
+    2416       70813 :           0, 0,   0,           0,           0, 0,   0,           1,           0, 0,   0,           0,
+    2417       70813 :           0, 0,   0,           0,           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,
+    2418       70813 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1,
+    2419       70813 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   1,           dt1,
+    2420       70813 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   0,           1;
+    2421             : 
+    2422       70813 :       B_ << 0,   0,   0,
+    2423       70813 :             0,   0,   0,
+    2424       70813 :             0,   0,   0,
+    2425       70813 :             dt1, 0,   0,
+    2426       70813 :             0,   0,   0,
+    2427       70813 :             0,   0,   0,
+    2428       70813 :             0,   0,   0,
+    2429       70813 :             0,   dt1, 0,
+    2430       70813 :             0,   0,   0,
+    2431       70813 :             0,   0,   0,
+    2432       70813 :             0,   0,   0,
+    2433       70813 :             0,   0,   dt1;
+    2434             : 
+    2435       70813 :       A_heading_ << 1, dt1, 0.5*dt1*dt1, 0,
+    2436       70813 :                     0, 1,   dt1,         0.5*dt1*dt1,
+    2437       70813 :                     0, 0,   1,           dt1,
+    2438       70813 :                     0, 0,   0,           1;
+    2439             : 
+    2440       70813 :       B_heading_ << 0,
+    2441       70813 :                     0,
+    2442      141626 :                     0,
+    2443       70813 :                     dt1;
+    2444             : 
+    2445       70813 :     model_iteration_last_time_ = ros::Time::now();
+    2446             :   }
+    2447             : 
+    2448             :   {
+    2449      141794 :     auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2450      141794 :     auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    2451             : 
+    2452      141794 :     MatrixXd new_mpc_x         = A_ * mpc_x + B_ * mpc_u;
+    2453      141794 :     MatrixXd new_mpc_x_heading = A_heading_ * mpc_x_heading + B_heading_ * mpc_u_heading;
+    2454             : 
+    2455             :     // | --------------- check the state difference --------------- |
+    2456             :     {
+    2457       70897 :       auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    2458             : 
+    2459       70897 :       bool problem = false;
+    2460             : 
+    2461             :       // position
+    2462             : 
+    2463       70897 :       if (fabs((new_mpc_x(0) - mpc_x(0)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2464           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(0), new_mpc_x(0),
+    2465             :                   fabs((new_mpc_x(0) - mpc_x(0)) / dt1), constraints.horizontal_speed);
+    2466           0 :         problem = true;
+    2467             :       }
+    2468             : 
+    2469       70897 :       if (fabs((new_mpc_x(4) - mpc_x(4)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2470           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(4), new_mpc_x(4),
+    2471             :                   fabs((new_mpc_x(4) - mpc_x(4)) / dt1), constraints.horizontal_speed);
+    2472           0 :         problem = true;
+    2473             :     }
+    2474             : 
+    2475       70897 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) > 1.05 * constraints.vertical_ascending_speed) {
+    2476           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(8), new_mpc_x(8),
+    2477             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), constraints.vertical_ascending_speed);
+    2478           0 :         problem = true;
+    2479             :       }
+    2480             : 
+    2481       70897 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) < 1.05 * -constraints.vertical_descending_speed) {
+    2482           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(8), new_mpc_x(8),
+    2483             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), -constraints.vertical_descending_speed);
+    2484           0 :         problem = true;
+    2485             :       }
+    2486             : 
+    2487             :       /* if (fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1) > 1.2 * constraints.heading_speed) { */
+    2488             :       /*   ROS_DEBUG("[MpcTracker]: heading update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x_heading(0), new_mpc_x_heading(0), */
+    2489             :       /*             fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1), constraints.heading_speed); */
+    2490             :       /*   problem = true; */
+    2491             :       /* } */
+    2492             : 
+    2493             :       // velocity
+    2494             : 
+    2495       70897 :       if (fabs((new_mpc_x(1) - mpc_x(1)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2496           0 :         ROS_DEBUG("[MpcTracker]: horizontal vel x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(1), new_mpc_x(1),
+    2497             :                   fabs((new_mpc_x(1) - mpc_x(1)) / dt1), constraints.horizontal_acceleration);
+    2498           0 :         problem = true;
+    2499             :       }
+    2500             : 
+    2501       70897 :       if (fabs((new_mpc_x(5) - mpc_x(5)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2502           0 :         ROS_DEBUG("[MpcTracker]: horizontal vel y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(5), new_mpc_x(5),
+    2503             :                   fabs((new_mpc_x(5) - mpc_x(5)) / dt1), constraints.horizontal_acceleration);
+    2504           0 :         problem = true;
+    2505             :       }
+    2506             : 
+    2507       70897 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) > 1.05 * constraints.vertical_ascending_acceleration) {
+    2508           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(9), new_mpc_x(9),
+    2509             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), constraints.vertical_ascending_acceleration);
+    2510           0 :         problem = true;
+    2511             :       }
+    2512             : 
+    2513       70897 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) < 1.05 * -constraints.vertical_descending_acceleration) {
+    2514           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(9), new_mpc_x(9),
+    2515             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), -constraints.vertical_descending_acceleration);
+    2516           0 :         problem = true;
+    2517             :       }
+    2518             : 
+    2519             :       // acceleration
+    2520             : 
+    2521       70897 :       if (fabs((new_mpc_x(2) - mpc_x(2)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2522           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(2), new_mpc_x(2),
+    2523             :                   fabs((new_mpc_x(2) - mpc_x(2)) / dt1), constraints.horizontal_jerk);
+    2524           0 :         problem = true;
+    2525             :       }
+    2526             : 
+    2527       70897 :       if (fabs((new_mpc_x(6) - mpc_x(6)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2528           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc y update violates constraints: %.2f -> %.2f, = %.2f > %.2f", mpc_x(6), new_mpc_x(6),
+    2529             :                   fabs((new_mpc_x(6) - mpc_x(6)) / dt1), constraints.horizontal_jerk);
+    2530           0 :         problem = true;
+    2531             :       }
+    2532             : 
+    2533       70897 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) > 1.05 * constraints.vertical_ascending_jerk) {
+    2534           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(10), new_mpc_x(10),
+    2535             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), constraints.vertical_ascending_jerk);
+    2536           0 :         problem = true;
+    2537             :       }
+    2538             : 
+    2539       70897 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) < 1.05 * -constraints.vertical_descending_jerk) {
+    2540           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(10), new_mpc_x(10),
+    2541             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), -constraints.vertical_descending_jerk);
+    2542           0 :         problem = true;
+    2543             :       }
+    2544             : 
+    2545             :       // jerk
+    2546             : 
+    2547       70897 :       if (fabs((new_mpc_x(3) - mpc_x(3)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2548           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(3), new_mpc_x(3),
+    2549             :                   fabs((new_mpc_x(3) - mpc_x(3)) / dt1), constraints.horizontal_snap);
+    2550           0 :         problem = true;
+    2551             :       }
+    2552             : 
+    2553       70897 :       if (fabs((new_mpc_x(7) - mpc_x(7)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2554           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(7), new_mpc_x(7),
+    2555             :                   fabs((new_mpc_x(7) - mpc_x(7)) / dt1), constraints.horizontal_snap);
+    2556           0 :         problem = true;
+    2557             :       }
+    2558             : 
+    2559       70897 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) > 1.05 * constraints.vertical_ascending_snap) {
+    2560           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(11), new_mpc_x(11),
+    2561             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), constraints.vertical_ascending_snap);
+    2562           0 :         problem = true;
+    2563             :       }
+    2564             : 
+    2565       70897 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) < 1.05 * -constraints.vertical_descending_snap) {
+    2566           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(11), new_mpc_x(11),
+    2567             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), -constraints.vertical_descending_snap);
+    2568           0 :         problem = true;
+    2569             :       }
+    2570             : 
+    2571       70897 :       if (problem) {
+    2572           0 :         debugPrintState(0.001);
+    2573           0 :         debugPrintMPCResult(0.001);
+    2574             :       }
+    2575             :     }
+    2576             : 
+    2577             :     {
+    2578       70897 :       std::scoped_lock lock(mutex_mpc_x_);
+    2579             : 
+    2580       70897 :       mpc_x_         = new_mpc_x;
+    2581       70897 :       mpc_x_heading_ = new_mpc_x_heading;
+    2582             : 
+    2583       70897 :       mpc_x_heading_(0) = sradians::wrap(mpc_x_heading_(0));
+    2584             :     }
+    2585             :   }
+    2586       70897 : }
+    2587             : 
+    2588             : //}
+    2589             : 
+    2590             : // | -------------------- referece setting -------------------- |
+    2591             : 
+    2592             : /* //{ loadTrajectory() */
+    2593             : 
+    2594             : // method for setting desired trajectory
+    2595         737 : std::tuple<bool, std::string, bool> MpcTracker::loadTrajectory(const mrs_msgs::TrajectoryReference msg) {
+    2596             : 
+    2597         737 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2598             : 
+    2599             :   // copy the member variables
+    2600        1474 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    2601        1474 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2602             : 
+    2603        1474 :   std::stringstream ss;
+    2604             : 
+    2605             :   /* check the trajectory dt //{ */
+    2606             : 
+    2607             :   double trajectory_dt;
+    2608         737 :   if (msg.dt <= 1e-4) {
+    2609           0 :     trajectory_dt = 0.2;
+    2610           0 :     ROS_WARN_THROTTLE(10.0, "[MpcTracker]: the trajectory dt was not specified, assuming its the old 0.2 s");
+    2611         737 :   } else if (msg.dt < dt1) {
+    2612           0 :     trajectory_dt = 0.2;
+    2613           0 :     ss << std::setprecision(3) << "the trajectory dt (" << msg.dt << " s) is too small (smaller than the tracker's internal step size: " << dt1 << " s)";
+    2614           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2615           0 :     return std::tuple(false, ss.str(), false);
+    2616             :   } else {
+    2617         737 :     trajectory_dt = msg.dt;
+    2618             :   }
+    2619             : 
+    2620             :   //}
+    2621             : 
+    2622         737 :   int trajectory_size = msg.points.size();
+    2623             : 
+    2624             :   /* sanitize the time-ness of the trajectory //{ */
+    2625             : 
+    2626         737 :   int    trajectory_sample_offset    = 0;  // how many samples in past is the trajectory
+    2627         737 :   int    trajectory_subsample_offset = 0;  // how many simulation inner loops ahead of the first valid sample
+    2628         737 :   double trajectory_time_offset      = 0;  // how much time in past in [s]
+    2629             : 
+    2630             :   // btw, "trajectory_time_offset = trajectory_dt*trajectory_sample_offset + _dt1_*trajectory_subsample_offset" should hold
+    2631         737 :   if (msg.fly_now) {
+    2632             : 
+    2633         735 :     ros::Time trajectory_time = msg.header.stamp;
+    2634             : 
+    2635             :     // the desired time is 0 => the current time
+    2636             :     // the trajecoty is a single point => the current time
+    2637         735 :     if (trajectory_time == ros::Time(0) || int(msg.points.size()) == 1) {
+    2638             : 
+    2639           3 :       trajectory_time_offset = 0.0;
+    2640             : 
+    2641             :       // the desired time is specified
+    2642             :     } else {
+    2643             : 
+    2644         732 :       trajectory_time_offset = (ros::Time::now() - trajectory_time).toSec();
+    2645             : 
+    2646             :       // when the time offset is negative, thus in the future
+    2647             :       // just say it, but use it like its from the current time
+    2648         732 :       if (trajectory_time_offset < 0.0) {
+    2649             : 
+    2650           0 :         ROS_WARN_THROTTLE(1.0, "[MpcTracker]: received trajectory with timestamp in the future by %.3f s", -trajectory_time_offset);
+    2651             : 
+    2652           0 :         trajectory_time_offset = 0.0;
+    2653             :       }
+    2654             :     }
+    2655             : 
+    2656             :     // if the time offset is set, check if we need to "move the first idx"
+    2657         735 :     if (trajectory_time_offset > 0.0) {
+    2658             : 
+    2659             :       // calculate the offset in samples
+    2660           1 :       trajectory_sample_offset = int(floor(trajectory_time_offset / trajectory_dt));
+    2661             : 
+    2662             :       // and get the subsample offset, which will be used to initialize the interpolator
+    2663           1 :       trajectory_subsample_offset = int(floor(fmod(trajectory_time_offset, trajectory_dt) / dt1));
+    2664             : 
+    2665           1 :       ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: received trajectory with timestamp in the past by %.3f s",
+    2666             :                          trajectory_dt * trajectory_sample_offset + dt1 * trajectory_subsample_offset);
+    2667             : 
+    2668             :       // if the offset is larger than the number of points in the trajectory
+    2669             :       // the trajectory can not be used
+    2670           1 :       if (trajectory_sample_offset >= trajectory_size) {
+    2671             : 
+    2672           0 :         ss << "trajectory timestamp is too old (time difference = " << trajectory_time_offset << ")";
+    2673           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2674           0 :         return std::tuple(false, ss.str(), false);
+    2675             : 
+    2676             :       } else {
+    2677             : 
+    2678             :         // if the offset is larger than one trajectory sample,
+    2679             :         // offset the start
+    2680           1 :         if (trajectory_time_offset >= trajectory_dt) {
+    2681             : 
+    2682             :           // decrease the trajectory size
+    2683           0 :           trajectory_size -= trajectory_sample_offset;
+    2684             : 
+    2685           0 :           ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: offsetting trajectory by %d samples", trajectory_sample_offset);
+    2686             : 
+    2687             :         } else {
+    2688             : 
+    2689           1 :           trajectory_sample_offset = 0;
+    2690             :         }
+    2691             :       }
+    2692             :     }
+    2693             : 
+    2694             :   } else { // not fly_now
+    2695             : 
+    2696           2 :       trajectory_tracking_in_progress_ = false;
+    2697             :   }
+    2698             : 
+    2699             :   //}
+    2700             : 
+    2701         737 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory sample offset: %d", trajectory_sample_offset);
+    2702         737 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory subsample offset: %d", trajectory_subsample_offset);
+    2703             : 
+    2704             :   // after this, we should have the correct value of
+    2705             :   // * trajectory_size
+    2706             :   // * trajectory_sample_offset
+    2707             :   // * trajectory_subsample_offset
+    2708             : 
+    2709             :   /* copy the trajectory to a local variable //{ */
+    2710             : 
+    2711             :   // copy only the part from the first valid index
+    2712             : 
+    2713        1474 :   MatrixXd des_x_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2714        1474 :   MatrixXd des_y_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2715        1474 :   MatrixXd des_z_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2716        1474 :   MatrixXd des_heading_whole_trajectory = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2717             : 
+    2718       37924 :   for (int i = 0; i < trajectory_size; i++) {
+    2719             : 
+    2720       37187 :     des_x_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.x;
+    2721       37187 :     des_y_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.y;
+    2722       37187 :     des_z_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.z;
+    2723       37187 :     des_heading_whole_trajectory(i) = msg.points.at(trajectory_sample_offset + i).heading;
+    2724             :   }
+    2725             : 
+    2726             :   //}
+    2727             : 
+    2728             :   /* set looping //{ */
+    2729             : 
+    2730         737 :   bool loop = false;
+    2731             : 
+    2732         737 :   if (msg.loop) {
+    2733             : 
+    2734           0 :     double first_x = des_x_whole_trajectory(0);
+    2735           0 :     double first_y = des_y_whole_trajectory(0);
+    2736           0 :     double first_z = des_z_whole_trajectory(0);
+    2737           0 :     double first_hdg = des_heading_whole_trajectory(0);
+    2738             : 
+    2739           0 :     double last_x = des_x_whole_trajectory(trajectory_size - 1);
+    2740           0 :     double last_y = des_y_whole_trajectory(trajectory_size - 1);
+    2741           0 :     double last_z = des_z_whole_trajectory(trajectory_size - 1);
+    2742           0 :     double last_hdg = des_heading_whole_trajectory(trajectory_size - 1);
+    2743             : 
+    2744             :     // check whether the trajectory is loopable
+    2745           0 :     if (mrs_lib::geometry::dist(vec3_t(first_x, first_y, first_z), vec3_t(last_x, last_y, last_z)) < 1.0 && abs(sradians::diff(first_hdg, last_hdg)) < 0.2) {
+    2746             : 
+    2747           0 :       ROS_INFO_THROTTLE(1.0, "[MpcTracker]: looping enabled");
+    2748           0 :       loop = true;
+    2749             : 
+    2750             :     } else {
+    2751             : 
+    2752           0 :       ss << "can not loop trajectory, the first and last points are too far apart";
+    2753           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2754           0 :       return std::tuple(false, ss.str(), false);
+    2755             :     }
+    2756             : 
+    2757             :   } else {
+    2758             : 
+    2759         737 :     loop = false;
+    2760             :   }
+    2761             : 
+    2762             :   //}
+    2763             : 
+    2764             :   // by this time, the values of these should be set:
+    2765             :   // * loop
+    2766             : 
+    2767             :   /* add tail (the last point repeated to fill the prediction horizon) //{ */
+    2768             : 
+    2769         737 :   if (!loop) {
+    2770             : 
+    2771             :     // extend it so it has smooth ending
+    2772       30217 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2773             : 
+    2774       29480 :       des_x_whole_trajectory(i + trajectory_size)       = des_x_whole_trajectory(i + trajectory_size - 1);
+    2775       29480 :       des_y_whole_trajectory(i + trajectory_size)       = des_y_whole_trajectory(i + trajectory_size - 1);
+    2776       29480 :       des_z_whole_trajectory(i + trajectory_size)       = des_z_whole_trajectory(i + trajectory_size - 1);
+    2777       29480 :       des_heading_whole_trajectory(i + trajectory_size) = des_heading_whole_trajectory(i + trajectory_size - 1);
+    2778             :     }
+    2779             :   }
+    2780             : 
+    2781             :   //}
+    2782             : 
+    2783             :   // by this time, the values of these should be set correctly:
+    2784             :   // * trajectory_size
+    2785             :   // * des_x_whole_trajectory
+    2786             :   // * des_y_whole_trajectory
+    2787             :   // * des_z_whole_trajectory
+    2788             :   // * des_heading_whole_trajectory
+    2789             : 
+    2790             :   /* update the global variables //{ */
+    2791             : 
+    2792             :   {
+    2793        1474 :     std::scoped_lock lock(mutex_des_whole_trajectory_, mutex_des_trajectory_, mutex_trajectory_tracking_states_);
+    2794             : 
+    2795         737 :     des_whole_trajectory_id_ = msg.input_id;
+    2796             : 
+    2797         737 :     auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2798             : 
+    2799         737 :     trajectory_tracking_in_progress_ = msg.fly_now;
+    2800         737 :     trajectory_track_heading_        = msg.use_heading;
+    2801             : 
+    2802             :     // allocate the vectors
+    2803         737 :     des_x_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2804         737 :     des_y_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2805         737 :     des_z_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2806         737 :     des_heading_whole_trajectory_ = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2807             : 
+    2808       67404 :     for (int i = 0; i < trajectory_size + MPC_HORIZON_LENGTH; i++) {
+    2809             : 
+    2810       66667 :       (*des_x_whole_trajectory_)(i) = des_x_whole_trajectory(i);
+    2811       66667 :       (*des_y_whole_trajectory_)(i) = des_y_whole_trajectory(i);
+    2812       66667 :       (*des_z_whole_trajectory_)(i) = des_z_whole_trajectory(i);
+    2813             : 
+    2814       66667 :       if (trajectory_track_heading_) {
+    2815       66667 :         (*des_heading_whole_trajectory_)(i) = des_heading_whole_trajectory(i);
+    2816             :       } else {
+    2817           0 :         (*des_heading_whole_trajectory_).fill(mpc_x_heading(0, 0));
+    2818             :       }
+    2819             :     }
+    2820             : 
+    2821             :     // if we are tracking trajectory, copy the setpoint
+    2822         737 :     if (trajectory_tracking_in_progress_) {
+    2823             : 
+    2824         735 :       toggleHover(false);
+    2825             : 
+    2826             :       /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    2827             : 
+    2828       30135 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2829             : 
+    2830       29400 :         double first_time = dt1 + i * _dt2_ + trajectory_subsample_offset * dt1;
+    2831             : 
+    2832       29400 :         int first_idx  = floor(first_time / trajectory_dt);
+    2833       29400 :         int second_idx = first_idx + 1;
+    2834             : 
+    2835       29400 :         double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    2836             : 
+    2837       29400 :         if (trajectory_tracking_loop_) {
+    2838             : 
+    2839           0 :           if (second_idx >= trajectory_size) {
+    2840           0 :             second_idx -= trajectory_size;
+    2841             :           }
+    2842             : 
+    2843           0 :           if (first_idx >= trajectory_size) {
+    2844           0 :             first_idx -= trajectory_size;
+    2845             :           }
+    2846             :         } else {
+    2847             : 
+    2848       29400 :           if (second_idx >= trajectory_size) {
+    2849           5 :             second_idx = trajectory_size - 1;
+    2850             :           }
+    2851             : 
+    2852       29400 :           if (first_idx >= trajectory_size) {
+    2853           4 :             first_idx = trajectory_size - 1;
+    2854             :           }
+    2855             :         }
+    2856             : 
+    2857       29400 :         des_x_trajectory_(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    2858       29400 :         des_y_trajectory_(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    2859       29400 :         des_z_trajectory_(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    2860             : 
+    2861       29400 :         des_heading_trajectory_(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    2862             :       }
+    2863             : 
+    2864             :       //}
+    2865             :     }
+    2866             : 
+    2867         737 :     trajectory_size_             = trajectory_size;
+    2868         737 :     trajectory_current_time_     = 0;
+    2869         737 :     trajectory_set_              = true;
+    2870         737 :     trajectory_tracking_loop_    = loop;
+    2871         737 :     trajectory_dt_               = trajectory_dt;
+    2872         737 :     trajectory_count_++;
+    2873             :   }
+    2874             : 
+    2875             :   //}
+    2876             : 
+    2877         737 :   ROS_INFO_THROTTLE(1, "[MpcTracker]: finished setting trajectory with length %d", trajectory_size);
+    2878             : 
+    2879             :   /* publish the debugging topics of the post-processed trajectory //{ */
+    2880             : 
+    2881             :   {
+    2882             : 
+    2883        1474 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2884         737 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2885         737 :     debug_trajectory_out.header.frame_id = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2886             : 
+    2887             :     {
+    2888        1474 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2889             : 
+    2890       37924 :       for (int i = 0; i < trajectory_size; i++) {
+    2891             : 
+    2892       37187 :         geometry_msgs::Pose new_pose;
+    2893             : 
+    2894       37187 :         new_pose.position.x = (*des_x_whole_trajectory_)(i);
+    2895       37187 :         new_pose.position.y = (*des_y_whole_trajectory_)(i);
+    2896       37187 :         new_pose.position.z = (*des_z_whole_trajectory_)(i);
+    2897             : 
+    2898       37187 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(i));
+    2899             : 
+    2900       37187 :         debug_trajectory_out.poses.push_back(new_pose);
+    2901             :       }
+    2902             :     }
+    2903             : 
+    2904         737 :     pub_debug_processed_trajectory_poses_.publish(debug_trajectory_out);
+    2905             : 
+    2906        1474 :     visualization_msgs::MarkerArray msg_out;
+    2907             : 
+    2908        1474 :     visualization_msgs::Marker marker;
+    2909             : 
+    2910         737 :     marker.header.stamp     = ros::Time::now();
+    2911         737 :     marker.header.frame_id  = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2912         737 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    2913         737 :     marker.color.a          = 1;
+    2914         737 :     marker.scale.x          = 0.05;
+    2915         737 :     marker.color.r          = 1;
+    2916         737 :     marker.color.g          = 0;
+    2917         737 :     marker.color.b          = 0;
+    2918         737 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2919             : 
+    2920             :     {
+    2921        1474 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2922             : 
+    2923       37187 :       for (int i = 0; i < trajectory_size - 1; i++) {
+    2924             : 
+    2925       36450 :         geometry_msgs::Point point1;
+    2926             : 
+    2927       36450 :         point1.x = des_x_whole_trajectory(i);
+    2928       36450 :         point1.y = des_y_whole_trajectory(i);
+    2929       36450 :         point1.z = des_z_whole_trajectory(i);
+    2930             : 
+    2931       36450 :         marker.points.push_back(point1);
+    2932             : 
+    2933       36450 :         geometry_msgs::Point point2;
+    2934             : 
+    2935       36450 :         point2.x = des_x_whole_trajectory(i + 1);
+    2936       36450 :         point2.y = des_y_whole_trajectory(i + 1);
+    2937       36450 :         point2.z = des_z_whole_trajectory(i + 1);
+    2938             : 
+    2939       36450 :         marker.points.push_back(point2);
+    2940             :       }
+    2941             :     }
+    2942             : 
+    2943         737 :     msg_out.markers.push_back(marker);
+    2944             : 
+    2945         737 :     pub_debug_processed_trajectory_markers_.publish(msg_out);
+    2946             :   }
+    2947             : 
+    2948             :   //}
+    2949             : 
+    2950         737 :   publishDiagnostics();
+    2951             : 
+    2952        1474 :   return std::tuple(true, "trajectory loaded", false);
+    2953             : }
+    2954             : 
+    2955             : //}
+    2956             : 
+    2957             : /* //{ setSinglePointReference() */
+    2958             : 
+    2959             : // fill the des_*_trajectory based on a single point
+    2960         738 : void MpcTracker::setSinglePointReference(const double x, const double y, const double z, const double heading) {
+    2961             : 
+    2962        1476 :   std::scoped_lock lock(mutex_des_trajectory_);
+    2963             : 
+    2964         738 :   des_x_trajectory_.fill(x);
+    2965         738 :   des_y_trajectory_.fill(y);
+    2966         738 :   des_z_trajectory_.fill(z);
+    2967         738 :   des_heading_trajectory_.fill(heading);
+    2968         738 : }
+    2969             : 
+    2970             : //}
+    2971             : 
+    2972             : /* //{ setGoal() */
+    2973             : 
+    2974             : // set absolute goal
+    2975         249 : void MpcTracker::setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    2976             : 
+    2977         249 :   double desired_heading = sradians::wrap(heading);
+    2978             : 
+    2979         498 :   auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2980             : 
+    2981         249 :   if (!use_heading) {
+    2982           0 :     desired_heading = mpc_x_heading(0, 0);
+    2983             :   }
+    2984             : 
+    2985         249 :   trajectory_tracking_in_progress_ = false;
+    2986             : 
+    2987         249 :   setSinglePointReference(pos_x, pos_y, pos_z, desired_heading);
+    2988             : 
+    2989         249 :   publishDiagnostics();
+    2990         249 : }
+    2991             : 
+    2992             : //}
+    2993             : 
+    2994             : /* //{ setRelativeGoal() */
+    2995             : 
+    2996         489 : void MpcTracker::setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    2997             : 
+    2998         978 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2999             : 
+    3000         489 :   double abs_x = mpc_x(0, 0) + pos_x;
+    3001         489 :   double abs_y = mpc_x(4, 0) + pos_y;
+    3002         489 :   double abs_z = mpc_x(8, 0) + pos_z;
+    3003             : 
+    3004         489 :   double abs_heading = mpc_x_heading(0, 0);
+    3005             : 
+    3006         489 :   if (use_heading) {
+    3007           0 :     abs_heading += heading;
+    3008             :   }
+    3009             : 
+    3010         489 :   trajectory_tracking_in_progress_ = false;
+    3011             : 
+    3012         489 :   setSinglePointReference(abs_x, abs_y, abs_z, abs_heading);
+    3013             : 
+    3014         489 :   publishDiagnostics();
+    3015         489 : }
+    3016             : 
+    3017             : //}
+    3018             : 
+    3019             : /* toggleHover() //{ */
+    3020             : 
+    3021        1173 : void MpcTracker::toggleHover(bool in) {
+    3022             : 
+    3023        1173 :   if (in == false && hovering_in_progress_) {
+    3024             : 
+    3025          89 :     ROS_DEBUG("[MpcTracker]: stoppping the hover timer");
+    3026             : 
+    3027          89 :     timer_hover_.stop();
+    3028             : 
+    3029          89 :     hovering_in_progress_ = false;
+    3030             : 
+    3031        1084 :   } else if (in == true && !hovering_in_progress_) {
+    3032             : 
+    3033          89 :     ROS_DEBUG("[MpcTracker]: starting the hover timer");
+    3034             : 
+    3035          89 :     hovering_in_progress_ = true;
+    3036             : 
+    3037          89 :     timer_hover_.start();
+    3038             :   }
+    3039        1173 : }
+    3040             : 
+    3041             : //}
+    3042             : 
+    3043             : // | ------------------- trajectory tracking ------------------ |
+    3044             : 
+    3045             : /* startTrajectoryTrackingImpl() //{ */
+    3046             : 
+    3047           2 : std::tuple<bool, std::string> MpcTracker::startTrajectoryTrackingImpl(void) {
+    3048             : 
+    3049           4 :   std::stringstream ss;
+    3050             : 
+    3051           2 :   if (trajectory_set_) {
+    3052             : 
+    3053           2 :     toggleHover(false);
+    3054             : 
+    3055             :     {
+    3056           2 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3057             : 
+    3058           2 :       trajectory_tracking_in_progress_ = true;
+    3059           2 :       trajectory_current_time_ = 0;
+    3060             :     }
+    3061             : 
+    3062           2 :     publishDiagnostics();
+    3063             : 
+    3064           2 :     ss << "trajectory tracking started";
+    3065           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3066             : 
+    3067           2 :     return std::tuple(true, ss.str());
+    3068             : 
+    3069             :   } else {
+    3070             : 
+    3071           0 :     ss << "can not start trajectory tracking, the trajectory is not set";
+    3072           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3073             : 
+    3074           0 :     return std::tuple(false, ss.str());
+    3075             :   }
+    3076             : }
+    3077             : 
+    3078             : //}
+    3079             : 
+    3080             : /* resumeTrajectoryTrackingImpl() //{ */
+    3081             : 
+    3082           1 : std::tuple<bool, std::string> MpcTracker::resumeTrajectoryTrackingImpl(void) {
+    3083             : 
+    3084           2 :   std::stringstream ss;
+    3085             : 
+    3086           1 :   if (trajectory_set_) {
+    3087             : 
+    3088           1 :     toggleHover(false);
+    3089             : 
+    3090           1 :     int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    3091             : 
+    3092           1 :     if (trajectory_tracking_idx < (trajectory_size_ - 1)) {
+    3093             : 
+    3094             :       {
+    3095           2 :         std::scoped_lock lock(mutex_des_trajectory_);
+    3096             : 
+    3097           1 :         trajectory_tracking_in_progress_ = true;
+    3098             :       }
+    3099             : 
+    3100           1 :       ss << "trajectory tracking resumed";
+    3101           1 :       ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3102             : 
+    3103           1 :       publishDiagnostics();
+    3104             : 
+    3105           1 :       return std::tuple(true, ss.str());
+    3106             : 
+    3107             :     } else {
+    3108             : 
+    3109           0 :       ss << "can not resume trajectory tracking, trajectory is already finished";
+    3110           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3111             : 
+    3112           0 :       return std::tuple(false, ss.str());
+    3113             :     }
+    3114             : 
+    3115             :   } else {
+    3116             : 
+    3117           0 :     ss << "can not resume trajectory tracking, ther trajectory is not set";
+    3118           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3119             : 
+    3120           0 :     return std::tuple(false, ss.str());
+    3121             :   }
+    3122             : }
+    3123             : 
+    3124             : //}
+    3125             : 
+    3126             : /* stopTrajectoryTrackingImpl() //{ */
+    3127             : 
+    3128           1 : std::tuple<bool, std::string> MpcTracker::stopTrajectoryTrackingImpl(void) {
+    3129             : 
+    3130           1 :   std::stringstream ss;
+    3131             : 
+    3132           1 :   if (trajectory_tracking_in_progress_) {
+    3133             : 
+    3134           1 :     trajectory_tracking_in_progress_ = false;
+    3135             : 
+    3136           1 :     toggleHover(true);
+    3137             : 
+    3138           1 :     ss << "stopping trajectory tracking";
+    3139           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3140             : 
+    3141           1 :     publishDiagnostics();
+    3142             : 
+    3143             :   } else {
+    3144             : 
+    3145           0 :     ss << "can not stop trajectory tracking, already at stop";
+    3146           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3147             :   }
+    3148             : 
+    3149           2 :   return std::tuple(true, ss.str());
+    3150             : }
+    3151             : 
+    3152             : //}
+    3153             : 
+    3154             : /* gotoTrajectoryStartImpl() //{ */
+    3155             : 
+    3156           2 : std::tuple<bool, std::string> MpcTracker::gotoTrajectoryStartImpl(void) {
+    3157             : 
+    3158           4 :   std::stringstream ss;
+    3159             : 
+    3160           2 :   if (trajectory_set_) {
+    3161             : 
+    3162           2 :     toggleHover(false);
+    3163             : 
+    3164           2 :     trajectory_tracking_in_progress_ = false;
+    3165             : 
+    3166             :     {
+    3167           4 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3168             : 
+    3169           2 :       setGoal((*des_x_whole_trajectory_)(0), (*des_y_whole_trajectory_)(0), (*des_z_whole_trajectory_)(0), (*des_heading_whole_trajectory_)(0),
+    3170           2 :               trajectory_track_heading_);
+    3171             :     }
+    3172             : 
+    3173           2 :     publishDiagnostics();
+    3174             : 
+    3175           2 :     ss << "flying to the start of the trajectory";
+    3176           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3177             : 
+    3178           2 :     return std::tuple(true, ss.str());
+    3179             : 
+    3180             :   } else {
+    3181             : 
+    3182           0 :     ss << "can not fly to the start of the trajectory, the trajectory is not set";
+    3183           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3184             : 
+    3185           0 :     return std::tuple(false, ss.str());
+    3186             :   }
+    3187             : }
+    3188             : 
+    3189             : //}
+    3190             : 
+    3191             : // | ------------------------- support ------------------------ |
+    3192             : 
+    3193             : /* //{ publishDiagnostics() */
+    3194             : 
+    3195       21949 : void MpcTracker::publishDiagnostics(void) {
+    3196             : 
+    3197       43898 :   auto des_x_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_x_trajectory_);
+    3198       43898 :   auto des_y_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_y_trajectory_);
+    3199       43898 :   auto des_z_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_z_trajectory_);
+    3200       43898 :   auto des_heading_trajectory = mrs_lib::get_mutexed(mutex_des_trajectory_, des_heading_trajectory_);
+    3201             : 
+    3202       43898 :   mrs_msgs::MpcTrackerDiagnostics diagnostics;
+    3203             : 
+    3204       21949 :   diagnostics.header.stamp    = ros::Time::now();
+    3205       21949 :   diagnostics.header.frame_id = uav_state_.header.frame_id;
+    3206             : 
+    3207       21949 :   diagnostics.active = is_active_;
+    3208             : 
+    3209       21949 :   diagnostics.uav_name = _uav_name_;
+    3210             : 
+    3211       21949 :   diagnostics.collision_avoidance_active = collision_avoidance_enabled_;
+    3212       21949 :   diagnostics.avoiding_collision         = collision_avoidance_affecting_me_;
+    3213             : 
+    3214       21949 :   diagnostics.setpoint.position.x = des_x_trajectory(0, 0);
+    3215       21949 :   diagnostics.setpoint.position.y = des_y_trajectory(0, 0);
+    3216       21949 :   diagnostics.setpoint.position.z = des_z_trajectory(0, 0);
+    3217             : 
+    3218       21949 :   diagnostics.setpoint.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(0, 0));
+    3219             : 
+    3220       43898 :   std::stringstream ss;
+    3221             : 
+    3222             :   {
+    3223       43898 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    3224             : 
+    3225             :     // fill in if other UAVs are sending their trajectories
+    3226       21949 :     std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>::iterator u = other_uav_diagnostics_.begin();
+    3227             : 
+    3228       24102 :     while (u != other_uav_diagnostics_.end()) {
+    3229             : 
+    3230        2153 :       if (u->second.collision_avoidance_active) {
+    3231             : 
+    3232             :         // is the other's trajectory fresh enought?
+    3233        1427 :         if ((ros::Time::now() - u->second.header.stamp).toSec() < _collision_trajectory_timeout_) {
+    3234        1427 :           diagnostics.avoidance_active_uavs.push_back(u->first);
+    3235        1427 :           ss << u->first.c_str() << ", ";
+    3236             :         }
+    3237             :       }
+    3238             : 
+    3239        2153 :       u++;
+    3240             :     }
+    3241             :   }
+    3242             : 
+    3243       43898 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3244             : 
+    3245       21949 :   if (ss.str().length() > 0) {
+    3246        1427 :     ROS_DEBUG_STREAM_THROTTLE(5.0, "[MpcTracker]: getting avoidance trajectories: " << ss.str());
+    3247       44390 :   } else if (collision_avoidance_enabled_ &&
+    3248       23868 :       (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    3249       17030 :     ROS_DEBUG_THROTTLE(10.0, "[MpcTracker]: missing avoidance trajectories!");
+    3250             :   }
+    3251             : 
+    3252       21949 :   pub_diagnostics_.publish(diagnostics);
+    3253             : 
+    3254       43898 :   std_msgs::String string_msg;
+    3255             : 
+    3256       21949 :   if (diagnostics.avoidance_active_uavs.empty()) {
+    3257             : 
+    3258       20522 :     string_msg.data = "-id col_avoid I see: NOTHING";
+    3259             : 
+    3260             :   } else {
+    3261             : 
+    3262        1427 :     string_msg.data = "-id col_avoid I see: ";
+    3263             :   }
+    3264             : 
+    3265       21949 :   if (diagnostics.avoidance_active_uavs.size() <= 3) {
+    3266             : 
+    3267       23376 :     for (size_t i = 0; i < diagnostics.avoidance_active_uavs.size(); i++) {
+    3268        1427 :       if (i == 0) {
+    3269        1427 :         string_msg.data += diagnostics.avoidance_active_uavs.at(i);
+    3270             :       } else {
+    3271           0 :         string_msg.data += ", " + diagnostics.avoidance_active_uavs.at(i);
+    3272             :       }
+    3273             :     }
+    3274             : 
+    3275             :   } else {
+    3276             : 
+    3277           0 :     std::stringstream ss;
+    3278           0 :     ss << diagnostics.avoidance_active_uavs.size();
+    3279             : 
+    3280           0 :     string_msg.data += ss.str() + " UAVs";
+    3281             :   }
+    3282             : 
+    3283       21949 :   pub_status_string_.publish(string_msg);
+    3284       21949 : }
+    3285             : 
+    3286             : //}
+    3287             : 
+    3288             : /* debugPrintState() //{ */
+    3289             : 
+    3290           0 : void MpcTracker::debugPrintState(const double throttle) {
+    3291             : 
+    3292           0 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3293             : 
+    3294           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC internal state: pos [%.2f, %.2f, %.2f, %.2f]", mpc_x(0), mpc_x(4), mpc_x(8), mpc_x_heading(0));
+    3295           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC internal state: vel [%.2f, %.2f, %.2f, %.2f]", mpc_x(1), mpc_x(5), mpc_x(9), mpc_x_heading(1));
+    3296           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC internal state: acc [%.2f, %.2f, %.2f, %.2f]", mpc_x(2), mpc_x(6), mpc_x(10), mpc_x_heading(2));
+    3297           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC internal state: jerk [%.2f, %.2f, %.2f, %.2f]", mpc_x(3), mpc_x(7), mpc_x(11), mpc_x_heading(3));
+    3298           0 : }
+    3299             : 
+    3300             : //}
+    3301             : 
+    3302             : /* debugPrintMPCu() //{ */
+    3303             : 
+    3304           0 : void MpcTracker::debugPrintMPCResult(const double throttle) {
+    3305             : 
+    3306           0 :   auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    3307           0 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    3308             : 
+    3309           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC result: [%.2f, %.2f, %.2f, %.2f]", mpc_u(0), mpc_u(1), mpc_u(2), mpc_u_heading);
+    3310           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: snap constraint: hor: %.2f, ver asc: %.2f, vert desc: %.2f, heading: %.2f]", constraints.horizontal_snap,
+    3311             :                      constraints.vertical_ascending_snap, constraints.vertical_descending_snap, constraints.heading_snap);
+    3312           0 : }
+    3313             : 
+    3314             : //}
+    3315             : 
+    3316             : /* getCurrentTrajectoryIdx() //{ */
+    3317             : 
+    3318        8214 : int MpcTracker::getCurrentTrajectoryIdx() {
+    3319             : 
+    3320        8214 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3321        8214 :   auto trajectory_dt   = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_dt_);
+    3322             : 
+    3323        8214 :   return floor(trajectory_current_time / trajectory_dt);
+    3324             : }
+    3325             : 
+    3326             : //}
+    3327             : 
+    3328             : /* increaseCurrentTrajectoryTime() //{ */
+    3329             : 
+    3330       15372 : void MpcTracker::increaseCurrentTrajectoryTime(const double dt) {
+    3331             : 
+    3332       15372 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3333       15372 :   auto [trajectory_size, trajectory_dt] = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_, trajectory_dt_);
+    3334             : 
+    3335       15372 :   trajectory_current_time += dt;
+    3336             : 
+    3337       15372 :   const double trajectory_duration = trajectory_size * trajectory_dt;
+    3338             : 
+    3339             :   // if the tracking idx hits the end of the trajectory
+    3340       15372 :   if (trajectory_current_time >= trajectory_duration) {
+    3341             : 
+    3342           5 :     if (trajectory_tracking_loop_) {
+    3343             : 
+    3344             :       // reset the idx
+    3345           0 :       trajectory_current_time -= trajectory_duration;
+    3346             : 
+    3347           0 :       ROS_INFO("[MpcTracker]: trajectory looped");
+    3348             : 
+    3349             :     } else {
+    3350             : 
+    3351           5 :       trajectory_tracking_in_progress_ = false;
+    3352             : 
+    3353           5 :       ROS_INFO("[MpcTracker]: done tracking trajectory, current time in trajectory=%f, trajectory_duration=%f", trajectory_current_time, trajectory_duration);
+    3354             :     }
+    3355             :   }
+    3356             : 
+    3357       15372 :   if (trajectory_tracking_in_progress_) {
+    3358       15367 :     mrs_lib::set_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time, trajectory_current_time_);
+    3359             :   }
+    3360       15372 : }
+    3361             : 
+    3362             : //}
+    3363             : 
+    3364             : // --------------------------------------------------------------
+    3365             : // |                           timers                           |
+    3366             : // --------------------------------------------------------------
+    3367             : 
+    3368             : /* //{ timerDiagnostics() */
+    3369             : 
+    3370             : // published diagnostics in reguar intervals
+    3371       20428 : void MpcTracker::timerDiagnostics(const ros::TimerEvent& event) {
+    3372             : 
+    3373       20428 :   if (!is_initialized_)
+    3374           0 :     return;
+    3375             : 
+    3376       61284 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.1, event);
+    3377       61284 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3378             : 
+    3379       20428 :   publishDiagnostics();
+    3380             : }
+    3381             : 
+    3382             : //}
+    3383             : 
+    3384             : /* //{ timerMPC() */
+    3385             : 
+    3386       77111 : void MpcTracker::timerMPC(const ros::TimerEvent& event) {
+    3387             : 
+    3388       77111 :   if (odometry_reset_in_progress_) {
+    3389           0 :     ROS_ERROR("[MpcTracker]: mpc iteration tried run while reseting odometry");
+    3390           0 :     return;
+    3391             :   }
+    3392             : 
+    3393       77111 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    3394             : 
+    3395       77111 :   mrs_lib::AtomicScopeFlag unset_running(mpc_timer_running_);
+    3396             : 
+    3397       77111 :   bool started_with_invalid = mpc_result_invalid_;
+    3398             : 
+    3399       77111 :   if (!is_active_) {
+    3400           0 :     return;
+    3401             :   }
+    3402             : 
+    3403       77111 :   if (!is_initialized_) {
+    3404           0 :     return;
+    3405             :   }
+    3406             : 
+    3407      231333 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerMPC", 1.0 / dt1, 0.01, event);
+    3408      231333 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerMPC", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3409             : 
+    3410       77111 :   ros::Time     begin = ros::Time::now();
+    3411       77111 :   ros::Time     end;
+    3412       77111 :   ros::Duration interval;
+    3413             :   int           trajectory_id;
+    3414             : 
+    3415             :   // if we are tracking trajectory, copy the setpoint
+    3416       77111 :   if (trajectory_tracking_in_progress_) {
+    3417             : 
+    3418       30744 :     MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    3419       30744 :     VectorXd des_x_whole_trajectory, des_y_whole_trajectory, des_z_whole_trajectory, des_heading_whole_trajectory;
+    3420             :     double   trajectory_dt;
+    3421             :     int      trajectory_size;
+    3422             :     {
+    3423       15372 :       std::scoped_lock lock(mutex_des_trajectory_, mutex_des_whole_trajectory_);
+    3424             : 
+    3425       15372 :       des_x_trajectory       = des_x_trajectory_;
+    3426       15372 :       des_y_trajectory       = des_y_trajectory_;
+    3427       15372 :       des_z_trajectory       = des_z_trajectory_;
+    3428       15372 :       des_heading_trajectory = des_heading_trajectory_;
+    3429             : 
+    3430       15372 :       des_x_whole_trajectory       = *des_x_whole_trajectory_;
+    3431       15372 :       des_y_whole_trajectory       = *des_y_whole_trajectory_;
+    3432       15372 :       des_z_whole_trajectory       = *des_z_whole_trajectory_;
+    3433       15372 :       des_heading_whole_trajectory = *des_heading_whole_trajectory_;
+    3434             : 
+    3435       15372 :       trajectory_size = trajectory_size_;
+    3436       15372 :       trajectory_dt   = trajectory_dt_;
+    3437             : 
+    3438       15372 :       trajectory_id = des_whole_trajectory_id_;
+    3439             :     }
+    3440             : 
+    3441             :     /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    3442             : 
+    3443       15372 :     const double dt_from_last_update = (event.current_real - event.last_real).toSec();
+    3444             : 
+    3445       15372 :     if (dt_from_last_update > 0.0 && dt_from_last_update < 1.0) {
+    3446       15372 :       increaseCurrentTrajectoryTime(dt_from_last_update);
+    3447             :     } else {
+    3448           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: timerMpc(): dt from the last update is not normal, not iterating trajectory, dt=%.4f", dt_from_last_update);
+    3449             :     }
+    3450             : 
+    3451       15372 :     auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3452             : 
+    3453      630252 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3454             : 
+    3455      614880 :       double first_time = trajectory_current_time + double(i) * _dt2_;
+    3456             : 
+    3457      614880 :       int first_idx  = int(floor(first_time / trajectory_dt));
+    3458      614880 :       int second_idx = first_idx + 1;
+    3459             : 
+    3460      614880 :       double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    3461             : 
+    3462      614880 :       if (trajectory_tracking_loop_) {
+    3463             : 
+    3464           0 :         if (second_idx >= trajectory_size) {
+    3465           0 :           second_idx = second_idx % trajectory_size;
+    3466             :         }
+    3467             : 
+    3468           0 :         if (first_idx >= trajectory_size) {
+    3469           0 :           first_idx = first_idx % trajectory_size;
+    3470             :         }
+    3471             : 
+    3472             :       } else {
+    3473             : 
+    3474      614880 :         if (second_idx >= trajectory_size) {
+    3475       91479 :           second_idx = trajectory_size - 1;
+    3476             :         }
+    3477             : 
+    3478      614880 :         if (first_idx >= trajectory_size) {
+    3479       84243 :           first_idx = trajectory_size - 1;
+    3480             :         }
+    3481             :       }
+    3482             : 
+    3483      614880 :       if (first_idx < 0 || first_idx > (trajectory_size-1) || second_idx < 0 || second_idx > (trajectory_size-1)) {
+    3484           0 :         ROS_ERROR_THROTTLE(0.1, "[MpcTracker]: trying to index out range when interpolating the trajectory! first_idx=%d, second_idx=%d, trajectory_size=%d", first_idx, second_idx, trajectory_size);
+    3485           0 :         continue;
+    3486             :       }
+    3487             : 
+    3488      614880 :       des_x_trajectory(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    3489      614880 :       des_y_trajectory(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    3490      614880 :       des_z_trajectory(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    3491             : 
+    3492      614880 :       des_heading_trajectory(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    3493             :     }
+    3494             : 
+    3495             :     {
+    3496       30744 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3497             : 
+    3498       15372 :       des_x_trajectory_       = des_x_trajectory;
+    3499       15372 :       des_y_trajectory_       = des_y_trajectory;
+    3500       15372 :       des_z_trajectory_       = des_z_trajectory;
+    3501       15372 :       des_heading_trajectory_ = des_heading_trajectory;
+    3502             :     }
+    3503             : 
+    3504             :     //}
+    3505             : 
+    3506             :   } else {
+    3507             : 
+    3508       61739 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3509             : 
+    3510       61739 :     trajectory_id = des_whole_trajectory_id_;
+    3511             :   }
+    3512             : 
+    3513       77111 :   manageConstraints();
+    3514             : 
+    3515       77111 :   calculateMPC();
+    3516             : 
+    3517       77111 :   end      = ros::Time::now();
+    3518       77111 :   interval = end - begin;
+    3519             : 
+    3520             :   // | ------------------ calculate the MPC RTF ----------------- |
+    3521             : 
+    3522       77111 :   mpc_rtf_ = 0.99 * mpc_rtf_ + 0.01 * (interval.toSec()/dt1);
+    3523             : 
+    3524       77111 :   if (mpc_rtf_ >= 1.0) {
+    3525           0 :     ROS_WARN_THROTTLE(5.0, "[MpcTracker] MPC Real Time Factor (%.3f) is slow", mpc_rtf_);
+    3526             :   }
+    3527             : 
+    3528             :   /* publish predicted future //{ */
+    3529             : 
+    3530             :   {
+    3531      154222 :     geometry_msgs::PoseArray debug_trajectory_out;
+    3532       77111 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    3533       77111 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    3534             : 
+    3535             :     {
+    3536      154222 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3537             : 
+    3538     3161550 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3539             : 
+    3540     3084440 :         geometry_msgs::Pose newPose;
+    3541             : 
+    3542     3084440 :         newPose.position.x = predicted_trajectory_(i * MPC_N_STATES);
+    3543     3084440 :         newPose.position.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3544     3084440 :         newPose.position.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3545             : 
+    3546             :         try {
+    3547     3084440 :           newPose.orientation = mrs_lib::AttitudeConverter(0, 0, predicted_heading_trajectory_(i * MPC_N_STATES));
+    3548           0 :         } catch (...) {
+    3549           0 :           ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: failed to fill orientation into debug print trajectory");
+    3550             :         }
+    3551             : 
+    3552     3084440 :         debug_trajectory_out.poses.push_back(newPose);
+    3553             :       }
+    3554             :     }
+    3555             : 
+    3556       77111 :     ph_predicted_trajectory_debugging_.publish(debug_trajectory_out);
+    3557             :   }
+    3558             : 
+    3559             :   //}
+    3560             : 
+    3561             :   /* publish full state prediction //{ */
+    3562             : 
+    3563             :   {
+    3564      154222 :     mrs_msgs::MpcPredictionFullState prediction_fs_out;
+    3565       77111 :     prediction_fs_out.header.stamp    = ros::Time::now();
+    3566       77111 :     prediction_fs_out.header.frame_id = uav_state_.header.frame_id;
+    3567             : 
+    3568       77111 :     ros::Time stamp = prediction_fs_out.header.stamp;
+    3569             : 
+    3570       77111 :     prediction_fs_out.input_id = trajectory_id;
+    3571             : 
+    3572             :     {
+    3573      154222 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3574             : 
+    3575     3161550 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3576             : 
+    3577     3084440 :         if (i == 0) {
+    3578       77111 :           stamp += ros::Duration(0.01);
+    3579             :         } else {
+    3580     3007329 :           stamp += ros::Duration(0.2);
+    3581             :         }
+    3582             : 
+    3583     3084440 :         prediction_fs_out.stamps.push_back(stamp);
+    3584             : 
+    3585             :         {  // position
+    3586     3084440 :           geometry_msgs::Point point;
+    3587             : 
+    3588     3084440 :           point.x = predicted_trajectory_(i * MPC_N_STATES);
+    3589     3084440 :           point.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3590     3084440 :           point.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3591             : 
+    3592     3084440 :           prediction_fs_out.position.push_back(point);
+    3593             :         }
+    3594             : 
+    3595             :         {  // velocity
+    3596     3084440 :           geometry_msgs::Vector3 vector;
+    3597             : 
+    3598     3084440 :           vector.x = predicted_trajectory_(i * MPC_N_STATES + 1);
+    3599     3084440 :           vector.y = predicted_trajectory_(i * MPC_N_STATES + 5);
+    3600     3084440 :           vector.z = predicted_trajectory_(i * MPC_N_STATES + 9);
+    3601             : 
+    3602     3084440 :           prediction_fs_out.velocity.push_back(vector);
+    3603             :         }
+    3604             : 
+    3605             :         {  // acceleration
+    3606     3084440 :           geometry_msgs::Vector3 vector3;
+    3607             : 
+    3608     3084440 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 2);
+    3609     3084440 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 6);
+    3610     3084440 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 10);
+    3611             : 
+    3612     3084440 :           prediction_fs_out.acceleration.push_back(vector3);
+    3613             :         }
+    3614             : 
+    3615             :         {  // jerk
+    3616     3084440 :           geometry_msgs::Vector3 vector3;
+    3617             : 
+    3618     3084440 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 3);
+    3619     3084440 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 7);
+    3620     3084440 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 11);
+    3621             : 
+    3622     3084440 :           prediction_fs_out.jerk.push_back(vector3);
+    3623             :         }
+    3624             : 
+    3625             :         {
+    3626             :           // heading
+    3627             : 
+    3628     3084440 :           prediction_fs_out.heading.push_back(predicted_heading_trajectory_(i * MPC_N_STATES));
+    3629     3084440 :           prediction_fs_out.heading_rate.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 1));
+    3630     3084440 :           prediction_fs_out.heading_acceleration.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 2));
+    3631     3084440 :           prediction_fs_out.heading_jerk.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 3));
+    3632             :         }
+    3633             :       }
+    3634             :     }
+    3635             : 
+    3636             :     {
+    3637      154222 :       std::scoped_lock lock(mutex_prediction_full_state_);
+    3638       77111 :       prediction_full_state_ = prediction_fs_out;
+    3639             :     }
+    3640             :   }
+    3641             : 
+    3642             :   //}
+    3643             : 
+    3644       77111 :   mpc_computed_ = true;
+    3645             : 
+    3646       77111 :   if (started_with_invalid) {
+    3647             : 
+    3648           5 :     mpc_result_invalid_ = false;
+    3649             : 
+    3650           5 :     ROS_INFO("[MpcTracker]: calculated the first MPC result after invalidation");
+    3651             :   }
+    3652             : }
+    3653             : 
+    3654             : //}
+    3655             : 
+    3656             : /* timerVelocityTracking() //{ */
+    3657             : 
+    3658         734 : void MpcTracker::timerVelocityTracking(const ros::TimerEvent& event) {
+    3659             : 
+    3660         734 :   if (!is_initialized_) {
+    3661           3 :     return;
+    3662             :   }
+    3663             : 
+    3664         734 :   if (!velocity_tracking_active_) {
+    3665           0 :     return;
+    3666             :   }
+    3667             : 
+    3668        1468 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerVelocityTracking", int(30.0), 0.01, event);
+    3669             :   mrs_lib::ScopeTimer timer =
+    3670        1468 :       mrs_lib::ScopeTimer("MpcTracker::timerVelocityTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3671             : 
+    3672             :   // stop the timer when timeout
+    3673         734 :   if ((ros::Time::now() - velocity_reference_time_).toSec() > 0.5) {
+    3674             : 
+    3675           3 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: velocity reference timeouted, hovering");
+    3676           3 :     timer_velocity_tracking_.stop();
+    3677             : 
+    3678           3 :     toggleHover(true);
+    3679             : 
+    3680           3 :     velocity_tracking_active_ = false;
+    3681             : 
+    3682           3 :     return;
+    3683             :   }
+    3684             : 
+    3685        1462 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3686         731 :   auto velocity_reference     = mrs_lib::get_mutexed(mutex_velocity_reference_, velocity_reference_);
+    3687             : 
+    3688        1462 :   mrs_msgs::TrajectoryReference trajectory;
+    3689             : 
+    3690         731 :   trajectory.fly_now         = true;
+    3691         731 :   trajectory.use_heading     = true;
+    3692         731 :   trajectory.dt              = 0.2;
+    3693         731 :   trajectory.header.stamp    = ros::Time::now();
+    3694         731 :   trajectory.header.frame_id = "";
+    3695             : 
+    3696         731 :   double x       = mpc_x(0, 0);
+    3697         731 :   double y       = mpc_x(4, 0);
+    3698         731 :   double z       = mpc_x(8, 0);
+    3699         731 :   double heading = mpc_x_heading(0, 0);
+    3700             : 
+    3701       37281 :   for (int i = 0; i < 50; i++) {
+    3702             : 
+    3703       36550 :     mrs_msgs::Reference reference;
+    3704       36550 :     reference.position.x = x;
+    3705       36550 :     reference.position.y = y;
+    3706       36550 :     reference.position.z = z;
+    3707       36550 :     reference.heading    = heading;
+    3708             : 
+    3709       36550 :     trajectory.points.push_back(reference);
+    3710             : 
+    3711       36550 :     x += velocity_reference.velocity.x * trajectory.dt;
+    3712       36550 :     y += velocity_reference.velocity.y * trajectory.dt;
+    3713       36550 :     z += velocity_reference.velocity.z * trajectory.dt;
+    3714             : 
+    3715       36550 :     if (velocity_reference.use_altitude) {
+    3716           0 :       z = velocity_reference.altitude;
+    3717             :     }
+    3718             : 
+    3719       36550 :     if (velocity_reference.use_heading_rate) {
+    3720       33300 :       heading += velocity_reference.heading_rate * trajectory.dt;
+    3721        3250 :     } else if (velocity_reference.use_heading) {
+    3722           0 :       heading = velocity_reference.heading;
+    3723             :     }
+    3724             :   }
+    3725             : 
+    3726        1462 :   auto [success, message, modified] = loadTrajectory(trajectory);
+    3727             : }
+    3728             : 
+    3729             : //}
+    3730             : 
+    3731             : /* //{ timerAvoidanceTrajectory() */
+    3732             : 
+    3733        4054 : void MpcTracker::timerAvoidanceTrajectory(const ros::TimerEvent& event) {
+    3734             : 
+    3735        4054 :   if (!is_active_) {
+    3736        1993 :     return;
+    3737             :   }
+    3738             : 
+    3739        2097 :   if (!is_initialized_) {
+    3740           0 :     return;
+    3741             :   }
+    3742             : 
+    3743        2097 :   if (!sh_estimation_diag_.hasMsg()) {
+    3744           0 :     return;
+    3745             :   } else {
+    3746             :     // we won't try to transform and publish the avoidance prediction if we cannot transform it
+    3747             : 
+    3748        2097 :     auto                     estimation_diag      = sh_estimation_diag_.getMsg();
+    3749        2097 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    3750             : 
+    3751        2097 :     bool got_gps_est = std::find(state_estimators.begin(), state_estimators.end(), "gps_garmin") != state_estimators.end() || std::find(state_estimators.begin(), state_estimators.end(), "gps_baro") != state_estimators.end();
+    3752        2097 :     bool got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    3753             : 
+    3754        2097 :     if (!got_gps_est && !got_rtk_est) {
+    3755          36 :       return;
+    3756             :     }
+    3757             :   }
+    3758             : 
+    3759        4122 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerAvoidanceTrajectory", _avoidance_trajectory_rate_, 0.1, event);
+    3760             :   mrs_lib::ScopeTimer timer =
+    3761        4122 :       mrs_lib::ScopeTimer("MpcTracker::timerAvoidanceTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3762             : 
+    3763        2061 :   auto uav_state            = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3764        2061 :   auto predicted_trajectory = mrs_lib::get_mutexed(mutex_predicted_trajectory_, predicted_trajectory_);
+    3765             : 
+    3766        2061 :   if (future_was_predicted_) {
+    3767             : 
+    3768        2061 :     mrs_msgs::FutureTrajectory avoidance_trajectory;
+    3769             : 
+    3770             :     // fill last trajectory with initial data
+    3771        2061 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3772        2061 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3773        2061 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3774        2061 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk");
+    3775        2061 :     avoidance_trajectory.points.clear();
+    3776        2061 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3777        2061 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3778        2061 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3779        2061 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_;
+    3780             : 
+    3781        4122 :     auto res = common_handlers_->transformer->getTransform(uav_state.header.frame_id, "utm_origin", ros::Time::now());
+    3782             : 
+    3783        2061 :     if (!res) {
+    3784             : 
+    3785           0 :       std::string message = "[MpcTracker]: can not transform predicted future to utm_origin";
+    3786           0 :       ROS_WARN_STREAM_ONCE(message);
+    3787           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3788           0 :       return;
+    3789             : 
+    3790             :     } else {
+    3791             : 
+    3792        4122 :       geometry_msgs::TransformStamped tf = res.value();
+    3793             : 
+    3794       84501 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3795             : 
+    3796             :         // original point
+    3797      164880 :         geometry_msgs::PoseStamped original_point;
+    3798             : 
+    3799       82440 :         original_point.header.stamp    = ros::Time::now();
+    3800       82440 :         original_point.header.frame_id = uav_state.header.frame_id;
+    3801             : 
+    3802       82440 :         original_point.pose.position.x = predicted_trajectory(i * MPC_N_STATES);
+    3803       82440 :         original_point.pose.position.y = predicted_trajectory(i * MPC_N_STATES + 4);
+    3804       82440 :         original_point.pose.position.z = predicted_trajectory(i * MPC_N_STATES + 8);
+    3805             : 
+    3806       82440 :         original_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    3807             : 
+    3808      164880 :         auto res = common_handlers_->transformer->transform(original_point, tf);
+    3809             : 
+    3810       82440 :         if (res) {
+    3811             : 
+    3812       82440 :           mrs_msgs::FuturePoint new_point;
+    3813             : 
+    3814       82440 :           new_point.x = res.value().pose.position.x;
+    3815       82440 :           new_point.y = res.value().pose.position.y;
+    3816       82440 :           new_point.z = res.value().pose.position.z;
+    3817             : 
+    3818       82440 :           avoidance_trajectory.points.push_back(new_point);
+    3819             : 
+    3820             :         } else {
+    3821             : 
+    3822           0 :           std::string message = "[MpcTracker]: can not transform a point of a future trajectory";
+    3823           0 :           ROS_WARN_STREAM_ONCE(message);
+    3824           0 :           ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3825             :         }
+    3826             :       }
+    3827             :     }
+    3828             : 
+    3829        2061 :     ph_avoidance_trajectory_.publish(avoidance_trajectory);
+    3830             :   }
+    3831             : }
+    3832             : 
+    3833             : //}
+    3834             : 
+    3835             : /* timerHover() //{ */
+    3836             : 
+    3837         404 : void MpcTracker::timerHover(const ros::TimerEvent& event) {
+    3838             : 
+    3839         808 :   MatrixXd mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    3840             : 
+    3841        1212 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerHover", 10, 0.01, event);
+    3842        1212 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerHover", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3843             : 
+    3844         404 :   setRelativeGoal(0, 0, 0, 0, false);
+    3845             : 
+    3846         404 :   if (fabs(mpc_x(1, 0)) < 0.1 && fabs(mpc_x(5, 0)) < 0.1 && fabs(mpc_x(9, 0)) < 0.1) {
+    3847             : 
+    3848          57 :     toggleHover(false);
+    3849             : 
+    3850          57 :     ROS_INFO("[MpcTracker]: timerHover: speed is low, stopping hover timer");
+    3851             :   }
+    3852         404 : }
+    3853             : 
+    3854             : //}
+    3855             : 
+    3856             : }  // namespace mpc_tracker
+    3857             : 
+    3858             : }  // namespace mrs_uav_trackers
+    3859             : 
+    3860             : #include <pluginlib/class_list_macros.h>
+    3861          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::mpc_tracker::MpcTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..9783d13ae2 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html @@ -0,0 +1,986 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..bb8f2b45954f119adea836686a64020b629df3c4 GIT binary patch literal 12420 zcmV-~FniC5P)%odUzm@TwBnC^cow~0ScQ1H`q&aLm=o@q-_5D^jZ@m!wI=lcD> z$Efy=??U>lp^Si4P<7Y7tx~|=AtV@?e#x%;I?ge&eG^oR0co@(AceFCBTWy9k)vGH zFvc!aBt~8)FrXNtylWjF#Tcc4(lJVjDG@Gf5Mw{K6r+~mJaB8*V!*DBzcF_GHHmS# z2Dywwe9~x1tsn(}!@I6v6$7?)J%pQc=6dK5|Q9ZEl zjF=8w4&!*~fg)Z49NshNx`KN`*Na^>*9gX7bflcK4V2LLH){$goy54!1cXb`P|SYC z0SdbI{lV@DOEJO9uNnrhgZTu7>mwcL@{Ci1AoGtY;0n7*yDnn%BTx~OWAqpTH7i4m z2L|&~1biaEt}(7T?GpC5y+r`HNC8hs0pRM!*gG>}a1N~op|t-vWW{(T7z@n}uMW7X z`Iyeevh)!K&<)xl?1V0Lj784Cqb1Echo#9d;3p)(C>vu_*EP>n__gSiE$iCP%C*5( z0iSLs?(O%)h%CY-VPCetOEBVji_=E}5`WiD7TGm5LtNMGvzc|u%_T9q&bh=hR`x3n zJbI8A3w|8g81-GRqfT;m(ge8dq_xWmiE+?+1{k05!_(RAi*ub60T=7-zrK=y=TAi# zJzxQ2vJ~iTGl5VgZo}Sky9Agcms zn`Z(RV8P<*o@IwIu;=3S;!nnIdT`f+eaP@6ZojOg#f#+nSs23)0;aI!+P!$T?6vdWc!AP?nF1-{V=}$kHi>V_YBw zOQv)JAX@&s(bEFxyJi4aY^r|~vv{Q&-2S?=!Xn(SuP!C3{ppO4>hyY%?h%6oqunh| z9+W-fn3#B2-~q1>9{Q6N4(3q+-WEHGqv4XKcr zq_G=l`V-l~LDMxL31jXDEN`KLG5T|G|6#>$XaldWG9N(VosI|0EdUyj=sNULy2`|-r7_b+C|(W0zj*Q*AfuL(SOR0OGc|Q+U=DS7;-X&zw^a%##h7aw8nBl= z{$dr)QUiOIz9E${mhRaS#Z$eKQ*^y}>x%qDkC6Z-Y+Q6+4*#mgmUkqHWQ40k{jTY@_aCWDb9TG_@_J|2xrTQ z;Yo}?Q7}l>X5{WIfOYYU|7p|pjNdp0jqy1412*G7?D!Xt4Yh-9@~*eTn8R$<_Naq+ zKtG5rkWm3N0gxPsw(A}7j6&H!3{Qs=9 z!e`txk*A_-3X4qvsF-W3BHUM#q6&ZtjODgBjFD?-yb3^h*E>dHXoa2$?q~ewTX;WT zuk$B23+{F)aHPYB>f8%cK}#Cwngukvu0v5k>uBL!EO=?{39UzvD9mvCWw%<#AWvGV zq!NlCd$h#(7-g28TuDILxON{;q|yMpcFqjY!O1pQuJ%X(IH4b!TdgNZOP)EcDuC?zJ|bb<;3J=bJg zCJVIsqt>+q(10Hy39Yf;9MG1O#+}o*hh%u_Y@HB9~k5=@NW_LTHelLW(bi8Me)qH7LLN636pd#vF=m#PA~=*@{zs{mGCbS_p07)zRIS3n;ZNy4lXK8toil?^ zxtjqptCrk`S)ov~2N`qEKK5`&!ae@eJjMY5eu>G&iW)n{gen6tUccus?y&0;;E@`d zl$aqvKePg1Bf}RE$=JzoQ@-$N^n7@d`!VjtYbFOMJV_~zQd4T~yVKbyj98qol)=^B z;<76rVcQ(%Y1~m8#*dG$&sPZc_;mk4-#YZLC*JrgGUR1Hy&|B{!|r|d03GR^pAnzFw0fFD-Q&QLYj zD8Us2%H*t{k69e)t?cjunxhUl`hLb+;*u$p(6VU(t3gk-C|;@LWVY2MW_5$h znb#gmwiH1-0Y%HuO1Qw@W4opRWnEuYj3jw?QJW_K?^6Q-uE?FUweOWCtk zXSod{_zA1*#k&H42^hP^*tK6%3!~9WDVTDTx8ZS@$m!$^!&r7+i4}mj>xl|r*mLk- zUy0w5aWN|HT_eV#%ZI23aAsKjuI#HoOvUqBB<7U?-bIYNN6lQ#U!LoJjMA>}yOxd& zFL)`xPDiDbkD6=gjB!4D@C#g1-&%o1Ivc?A8E9gt7LSp<8}7(($Qka^G^6+Xr5huC z>ypm`04z_@6hK+~H;MVEUm^!U3C7E@KMYXTpppW!!_G{dms0@M!~~5&>b6FVFIM$T zs!(;8(Q?`f|0CDu6;ZD)>0f#vK=V`8e{1T+XrxU*JWMcMTG4 z;Gr}->pctcdN?oPW24(MWVW!<=TjY6h0|mw)HQqCxP2wIyV)*SKD5+fAzpQm^jpx_P znspbT-7CwsI0_ixc2*B43mC9h=b)O9r#$i2psVweN0mxgGS@%XjkCG(#(mYIba`+X zZ%Z$F*hiVdMRa_uX^m$7Sqwc0PbCb&FNHcWlkbq+qG84;>>7sI#+qL!Pnjg)!V`OuD*#X?JV`~#pi#Vg&tDO}CMKPFr9#!myZ(a(i4U^k zo@+Yq3@ARwA1`2HY?Ag?}I2z;i z(;8s-sjzGEh&Ewk7dIwM;xp`xE>J>U(m?~Vt!OpT=zpxX}sP-Q%D`` zr3zI^@&%eeKI=_ou)ENQwNii95wq0ZxE1;{wujk4re(=_CFl3NFxyd4whtU}fN#d} zTuZ00<+ykLA${C4y5k;vDA_sMNwRyR;;E_}LiTJ&T6muGe9QpMr-ahDKN65nbt{9R}W(^)Jjmz5h~QnltsKfX`JvLK-RzCtlR(oI*m{rvUbhF{=!qdhn+x zW^pM_PC6}Oe5<6>VfJtme(R<-|3SlsG`bWj4E$qjH3>1oTYD&g1fa7Wb{IWo7lVmA z(w!%EI7Van4mlO})7f!*rc@6AvaQs01RR;NOkH57KYJGi5KC7NiOUx$fTR7az)xkG zo?ZY{bbV=teETzQp#s3TI!NmEUDsqbV2l!I7w&%eDAL{k&|HZPVk;l{s)@$*J9Ts(z9^7)J3R=a1nn}{vx z5_R@<$lY?;D`KokT5Ehynd+39gK|JPKvCCshR?@pZr-UHpVVl47r?QjSddHwxP=Uo z;ystJsSAw@UJ;er6nz=pI1=#s7y!#Rag**9{`fe8k8=A_*nm%}9Ju4DekC>SeWX|* z6#-TlE@Tfbe4huY`8Mf7EX0Pngj+5;?AQ|*J0Zzt7GV5^;r7Hfi`2OCYToiPA&E!j zyz013f%G=`%}_RA8BmDv<19#guK&+htlt3&7<@W)qml5SeJTY(93WK&CFrh~E-ocY zM0`=-cL9>u*#w}2{lc0H0{MoDZlt zngQ8#$SgE{cRm*aaCB(8uWlfy<06zEY6?>>`I)gqRwmH{nryaxN_4_U4Pe=?_)ZMZ zsm?C#G=SD`VCT)AGu_4pBE02eR5ZYzGJ~}2DIbtFHDI+^^knH-1Di;y<67Gbo>B}* zUc-C9IS$_mUM`su0bE|B;52u0q$ir#`2Vl37Q;AxcC>YD-Uy;Aj3F-F0G{p+2IIz@MV zcMtMAr1;xmcf+yxM&@N}#fYe=02-6@)B~s)_D)kX?9;6aPVV0V9ExcYJ8D!nnYZWFpb8lC%C zOEnWSI`(o){Ww>J23r7{IX7<8_V{NHqBjcJ2!;4ecjFFI82&fg@f({rV zxXEqvdkO9+;Vl?(h-*J=#Nk(W?t5T`H<0q{R3Uqq98m&Lk1=fdbXMiea)iYRZ)8u{ zUgL*Oj3ldB!0zk0#RL!%T(r79@dtfk5 z=}+WcPri1!FlQsxV`TWk0d8gAPy=8kKka{OUgMSEzxW^MJ4{{i{mV;c6wb4I1SZW) z=hc>I^Nec9kODac_XB2VgQggpX&0K^y{PR==dyF2IQNFe(>+M_jJZLlUyfiiGe%9< z&*%YMo-q$;K`-oVu9Qik?b?~yE3&O6#-~brfmrI#m|fxi4^mD-W7kXym<3Y@K;h#0 z4a+r%e-h-XW;j{k(8&Uv*=F;aafbC6?A(RZ6u`zY=F!MTUp2k9UUyd|Mvif>Wi>g# z?3}`~XE-2{NH7!QBrTe{{*d7&O- z#6u8$(CF@l?D;q(ZZ2b5_UjBU#MtPQDrT~v@K81*e`_>8h#-=9VWh%nU=d`Pq)HIajX_#=>Lz#30gbCAsv9G?apucz zT;Q-85K$H*vL_bK@S8@l>!2w-W1P*lg06}ACA(Hpy{tifxa<}8jg`VcOfoITfAIpS z?D}DuUF7(N$u_pc5`i%Q#v*X&2-L8y6S9*9W_&eqkC75tDmzsWSs-qC&+%?LeBL!y z%GKmnfcNk(&sa;jK1KtEYTOT;WiKhBa}80>Btg4I$eqQ3{>k~s=$|qmxB{A{q|k0p zUOs0t!xe#=Gc%*%r0t|w#PG?1#oI;wD`G>KOKGJbAh_kS6f3923vweyTj20bJAoj1ExR^&<*Zh2u8@N}I5l ztx}Pmr(DVV3TR4pCR?%{@cQVgd8VbTq=3S3JYtOOTa|5qV2pmAuIjoPz|oaCNmH0$ zm(HsY*XxFa(*B49jO4=gZM1;$F=pzFqdJA-Fp8$;&aO4hIR@B5tzLREVpys-0i2ub z@KmW`=_WAtr#8L=F*6i^PT9MG`Wnk78L$RWtC|tUBk8SJCA!(jQn|~9b>S~YR=TW@BFxVSC8;HfEawfe*9a9vtM$a+3|DM_$IgMn3654 zfcwtV2R6A1%k|w%u2IhYEb@V!YZ-fV&!LE(mAUh(^2RZ`LE1lLYZH1vuXl1l_gk&i z3~=QZ@17i+IY8saSOv7e2_W^Dl)=W)_Yc@^wGjPPw ztEuaabh;dwXwraJpB;dmV@$?bAO?4@0vs?#AZ-P}6Hg;x-x$+nO9J9f?N2m-6D%n} zf&Is)@W8JKuvBtSv0SC+p8Qo(=Vkk{DY?E%hCI{my&}%X75imE)|#h~Q=?rN$45V) zmI?XkXHn*T)U6b|X2e#C1KN0uJH$v9KnJ{Pj?myg$gMqYbL}WJ4$5)>rAQS*n+JF# z8a^$yF~Jx%FvgUNOppFY_e;Wj48#b>nE5lZY>3Rpty1m7ynJA>&l^aoR=nGc0*3tA zJ|||glL-AsT!{Z&9weVp;WI;6&rfD4FlT@X+pxIF#yIN$dBlJn+#R0len1Rd0W<(m zIy3lQfx|Zn5CT^LVSJC07`CX|Fu(-34A@pY@LwYaW1$oC8n+Lq#@p*Zl7l3xHKH6X zW3#mO!#xphK;Z6SP0eBMha^RVyz~yCZk6Y`Eyj`A@*`G5{^YJ*iJPl0s`Ga^HU*e;X z7;jTtJ(qOPJirPqT64xE;@^+4?8G^=83HoCS zmH#sYuoSl^22>J5%taB9{{sdv7at1HTXgZWb4sVD7&f0vM)i&FyYfpR1)Q`LiasGF zF%O%*9m;wvLF+9m)Pf;;wm!(4XC@0c$Ya!|nXdN!C$`*zP0VYb7SCT72mt5B&cw)Z z!dTG+l7n*gE9I)!x0;ROey+NUlUq`vzULeP{%nc?e0)EfVhEs;7-BApfczhDHU**0 z2p~T{$Dd8{CN{;Nk}9xAH#2y8{*+XTkz6MQK>iPIRZ_hnF)8LdblXVRw2xB60^Y#?z)?y_2Wk=a`(#jf4ywrt_A#X_s^v}&5v+VJ?8Qg6LbAT>Vsxt>e>6V zi20kDznS@)nHn+jZCIGGgsCo!B4tmB>i2w4lVnekc^L4B*Wu;6Vc)aArR4x82ArY; z9|$$}T}SV%an?^5E&Pso3Hn+sFCVV-`6+!R?|XsC2w9|ACKuq8l;y}DezsdqA3l+O zMWsetPam<-PXt)+HT|2!?c5KD|3+0M#$UUL07EC9^y{0}nUO}EhcQ-TH0Olv#M;Os z^3L^&G;rFs;>_6T0g*#R!0e>2ox*!lZ6$X;DvTRMR0u+B!Wd&4Nngr%K>Zl$T=%QB zVq~X!yED<|MmLT)NAyjyQ^QTc7>t>MFjl@h(gu(5xug&qe-3DNHL($k7_eXx-<9SSa#X%N1KD~5i{{>RPW~4!)1ac`~25->woZwKewkECypgR`*X`l z0id93444kOgb@w8-nAQa{}_KAprAeMpC4O5Q2;z)#{(Ysb*%tA%6U+~VvJt)%YgM3 zjA24ua2T_dR|MQFAekyb0G7KhH%Lqs(04r^P%?$}rr3tZn}ynooN^|DOKlVas@Su8 zhU~j_U}tA@2pIVoYU0JL4Ba*i9y?w_9!>t(vF1p+gYhNJvrD-Yybo_k63fHae^kg( zo$5&K?)~>6MAf&;t(dtVU2dt*m^?p5|6>IG^j|*#0*|r!Ui}Fu@^k-dRFA)@*b=8S zMKeP+LG)KfHBIyHnd|=#)erybX4XI!lso7>AL+BCYjBS{1#3_9?nqC)TOYATjLTB3 z%=ry7*az{a9|uU!AJzXKP4WMv`Vsv&sy~wP@&CDI{-0FG<4eA1+SiOL(UL8iKdSH1 z6eF#a-ZgXa=M{eN^?JKC^Z%r}Va;SjzsH*SpHp4Q97w4yhBe}_F9uXY%x$57n=lkm z>uWwg?nLBK1{n1Y2dsB4pluA-UW2HDBj_{2U@y`7D3?Co9B%s|;2XzfgNS>d4w^tzk z>k2Kt4E8VF;)9f*aDTFZp9t#wLUc{M?@5l)E8I)n7H?DT3(*p{jVi`L5+iK*VX#<6 zyF*zfg~qQaQe*3-dePr)aB%{7>I?wrG^RPyH$Tk=G5Wf!YO3)Ax`c1~doPM1S~c5D z-1P=3%AwfHW{hEzTdoR_VQ(VlCWIibPUX|k&&nZ}aoVZ($Sc^D7`@ir*(7OsQ`iS zdrGy8J=dCXM%N3OcysOQ$Bx4BqKg zYbJSSm63`MFf}jihH7Lu#25qv=;g1kSdm;jg@fntViHzUynd9J;@1#`oe$p?PTqLi zX$ZoEIbNInM5(_S#}rzFU*GIeiI)Lg%7$+H2ru72EwNJ0pXh6#!kREk{Kx&j86ghQv=W;d0X9Z+I^aRrAG+{RA{UUf<61+3DN^hxrvp;V z@qlljQsQB6ROHuY=k}=OEUWXmMDK{pFb2!)6PXFwVQ3~`0+|A9tQ3nedsoJF@&kZ0 zWCYe2Nbx0x;76u*htpgqI*jLL_gj^<2S*<9S=ti7T4@s{)uRpj4|kmTyxv~5l=#{d zpZ4GDbD{XlC!*T{)_uqmBBkd%ekUkd@E&HN4J}@vL&JMjGQhE+Gj98^58`+;Y6WXo zzq7cDoI1m0^Ab`-*HDdp<^4VKM=fPtZ;S^iwJzoZBG?4*#)mdj zIOIqiFWEx?s{i8}MK&Z|*9Fr#hTe=FK`h8{}L-`52QcUluSg!H{#^7Jm_K!ylOei4Oq& zRCE7jlYffEStbi{u*leW(8ta;=_&2A4cAxD%39*BrrRnMo~KP{_^7B3!3EcSk;_C* zrA~>_dM|T3#*ly^sJZjL5E6rkh?M~k^{u^Vz3Q&7{DC-z4&{M7?ns)zQLFI!Dp;^` zF#}_a*E6rY3ou3i*FDA=*DfwUoxXg8Q;6h+6iEvxFvXk37{g3>uPa_sRJ{n@7{_Yy zz(vRMSbH&vVV?siY5DHD(+15$#zmD^;l9FUk!Cw++HlQmT>j0`2xub4GN6O518#VX zM06M&VPR(8!7&8I+tKSk``f@>cjih8ZbF;G-_2`Jl;exk-cgzJY8oT%cU9&jZsQu& z-ptCJ)XdBP)Q|D6GH1fg4i^bde8B8!!MKy?S}_{n%^;#kQ2j^Ro8g24VxRz1j1I6q zMvsvX1#ZMR5b&Zsq1wc!tKL?T8k4KiW|eYqCWsy*R!__kU_78|jIgnQ4H&2Kktm)z z2~agg2_Jva_Q4Y)q&mY$%H_Km?1NJBV2+Q8U$ZqKPh-80J22_g#qJHj=mP71NA+$I7)TRLN zUen5O;U%d)z^@wO7xP_XyxAx1y_SU_X{t%2q%z?{t;-coFU^-?Wg@m{F-XxrN>fKH76gaT-E zDRV8ewYqajPr<#4PM{#BafuD~2HWYUW#lVD!}s_TDk0#WL z>Bm@l{jL#X5m0(|eUEhw-y4r%!aj1PsTdJz{Tc(Uzr4UuSmzC@d04d_5JFPiHe21? zBQW;6XWNe{bnImnkh_&W0i}`B(k52`!ggZZ6F-mS>Kvqrx<7B~I`uVm5;H#U17yX3 zEbdoK=5}=ov&u?LR!l>h7D%5VZB)7G`X2H4ST*;*M2FP`cg42&144PYgw<*;AF(H;~6Dsk1i(`~{AZ}Lga*j?ie(}z*$+-t&E1C(Rj zL-`gTz**_Kv?)ySQ%EsR)jf#@zjR8XDMY$wiVInAKzf_Y_@OQrk?zr!$DFBqezRpi zSexA(WtX=EIY=dAy95~D$Bkp0^WUHv-xx6Egq!jIi5tfmmA)J|uF(BwXpX|TB-;Lo z)PLUt0)w6qphC1dlXt;F*tjfqV=Jc313bvcf&i!>CjH~H^|2?mLRfs#b(#m<*R=~l zU;!J5DWYs=I6w`!MgX2aBtok?x<+Z;W-eeedzciOJFUGD)lC|2n_X-e}`hDVy7(G!_U6;>cnsyxFFP-Q#m6E@*5d#(aO>opiuP z#rNd>UKLn|=PQVVv}GMbja%G=JAHe$gLaG|ncPHyBeGQQJkKy=B#4h*z+_FFn&01Z?X|+L@Gj1f!!;35UA5?ZREaKM`f+pBq8Z2LKy-NXCfBzN2 zc0T>5)8BIgp#Ay~%B_Nr6QyUsO`&as^pNs1LwY=r{X zGduCL>XCYr8!r|MQAmx3CR{Tapd6zn;r*_e0koDY>-2nYy?%g+I(^hDSeVrmM&1DX&c)mwHVU__dXI_`~*brrro zp?ZFTb$y_ZjqV3ykV^wTAkPFB=gfQv?-e)x5kc#V2=DJr$miOG{xI!L?)V4M_Dpo` zH?fw84x|0!owCsg!1B8wZN!w`1?h^DD!k((!gz|1;9U^-*7>J<@!mrdfD+M_3vL`^ z@m&xw#@DMf#2I6J4ENb>ak5Jfi1>Ijw4zDbRmGcW$5?zo1dOr#fXMAJaZdC2*etLu zc-63*^hv9bGvMUYou1w8UhY|@1y>qHq97(}{}H538e%LqC^(HPGrQU@F6 zPnH$n2V+kk*Se%mG1x3DS(@H$@HOE)gJ{7mn?h-Mqo03k$V<=g-F4P&{OKoaekm<& zZHn|-E0}?Ct}{^}jZI49l1ZIfONzRiX)oY~N#^bY{?E!@kbU9R9tsO%0VQ5i3L_6V z!J$Dla*Wch*DKDXVi|3?Z`RV)P2rZVz3`0`aF5Z8N8I*R3;ii6u{rit2)e zG80fW#=PaH;b}8do||-xdpfvwsw*Ns5U^n{+zO^o0yt@AYC!22|L-$H08W@0G2~@z z>X7q|ry4e6gn)uv`!>MQ_X^_+^9-;bp`;}a-(o1oZ9&@8Udb!H!4TLZqZ-}177XI1 z(?{urjMnpvyREVhgX~9m+2YW|u)o1nh+~v>o!MJXT>T?K{#(L3!?Q=-bq%nE zn1YH_V!Uf{UF_#m3*B!~Ai3enza59sU)uS<^r#8;s4<@3l24}OT}uGnsG{9n#7JM^ z5P--rkpT*uP2SzyZDpZ;*et*U=sE^ib?+K{g(Z(%n>j;H2?5Bzq%#}P$qp-K!bV>} z^ewI}D_}!9rl$bPsm^v?N`$Vcrljfuch5B;)|Idf@=-bqAg>yf^60TqrM?K1F2Xk< zoQ_tMQa!J<+TqqKQ*-mJQu4;7iPA@aJTjHmO4}HVT@FYL^gs(zgfB9rW9jh+Q*Nr7~PRQWGlHa40^d9S%3K{ zBr^*Q1S}AP0YOJhq)P&5u9wMkFbN-j9WjSiD2OEStZ9{0t?;B~j341?a$p>Of~0$2 zC+;JClYlmw)4DSO3kO)=41ZUFnvf3AkxM!@Bto|e^3D| zcwIEzX%;Zz0@(t zOf$skMs0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
speed_tracker.cpp +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
<unnamed>14.8 %61 / 41136.8 %7 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/speed_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..0735378d45 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
speed_tracker.cpp +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
<unnamed>14.8 %61 / 41136.8 %7 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/index-detail.html b/mrs_uav_trackers/src/speed_tracker/index-detail.html new file mode 100644 index 0000000000..4b0e346c5f --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
speed_tracker.cpp +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
<unnamed>14.8 %61 / 41136.8 %7 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/index-sort-f.html b/mrs_uav_trackers/src/speed_tracker/index-sort-f.html new file mode 100644 index 0000000000..e0f82897fd --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
speed_tracker.cpp +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/index-sort-l.html b/mrs_uav_trackers/src/speed_tracker/index-sort-l.html new file mode 100644 index 0000000000..3638d1372b --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
speed_tracker.cpp +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/index.html b/mrs_uav_trackers/src/speed_tracker/index.html new file mode 100644 index 0000000000..dee9eeba89 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
speed_tracker.cpp +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..71c169d2b4 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::speed_tracker::SpeedTracker::resetStatic()0
mrs_uav_trackers::speed_tracker::SpeedTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::callbackCommand(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)0
mrs_uav_trackers::speed_tracker::SpeedTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::getStatus()0
mrs_uav_trackers::speed_tracker::SpeedTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()20
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::speed_tracker::SpeedTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html new file mode 100644 index 0000000000..30624505da --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()20
mrs_uav_trackers::speed_tracker::SpeedTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)94
mrs_uav_trackers::speed_tracker::SpeedTracker::resetStatic()0
mrs_uav_trackers::speed_tracker::SpeedTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)325
mrs_uav_trackers::speed_tracker::SpeedTracker::callbackCommand(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)0
mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)297
mrs_uav_trackers::speed_tracker::SpeedTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::speed_tracker::SpeedTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)127134
mrs_uav_trackers::speed_tracker::SpeedTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::getStatus()0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.frameset.html b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.frameset.html new file mode 100644 index 0000000000..8377e2d518 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html new file mode 100644 index 0000000000..54585b289b --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html @@ -0,0 +1,1178 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-04-25 05:42:57Functions:71936.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : 
+       5             : #include <mrs_uav_managers/tracker.h>
+       6             : 
+       7             : #include <mrs_msgs/SpeedTrackerCommand.h>
+       8             : #include <mrs_msgs/VelocityReferenceSrv.h>
+       9             : 
+      10             : #include <mrs_lib/profiler.h>
+      11             : #include <mrs_lib/mutex.h>
+      12             : #include <mrs_lib/attitude_converter.h>
+      13             : #include <mrs_lib/subscribe_handler.h>
+      14             : #include <mrs_lib/geometry/cyclic.h>
+      15             : #include <mrs_lib/geometry/misc.h>
+      16             : #include <mrs_lib/publisher_handler.h>
+      17             : 
+      18             : #include <visualization_msgs/Marker.h>
+      19             : #include <visualization_msgs/MarkerArray.h>
+      20             : 
+      21             : //}
+      22             : 
+      23             : /* defines //{ */
+      24             : 
+      25             : #define STOP_THR 1e-3
+      26             : 
+      27             : //}
+      28             : 
+      29             : /* using //{ */
+      30             : 
+      31             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      32             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      33             : 
+      34             : using radians  = mrs_lib::geometry::radians;
+      35             : using sradians = mrs_lib::geometry::sradians;
+      36             : 
+      37             : //}
+      38             : 
+      39             : namespace mrs_uav_trackers
+      40             : {
+      41             : 
+      42             : namespace speed_tracker
+      43             : {
+      44             : 
+      45             : /* //{ class SpeedTracker */
+      46             : 
+      47             : class SpeedTracker : public mrs_uav_managers::Tracker {
+      48             : public:
+      49             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      50             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      51             : 
+      52             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd);
+      53             :   void                          deactivate(void);
+      54             :   bool                          resetStatic(void);
+      55             : 
+      56             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState &uav_state, const mrs_uav_managers::Controller::ControlOutput &last_control_output);
+      57             :   const mrs_msgs::TrackerStatus             getStatus();
+      58             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd);
+      59             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      60             : 
+      61             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd);
+      62             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd);
+      63             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd);
+      64             : 
+      65             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      66             : 
+      67             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      68             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      69             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      70             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      71             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      72             : 
+      73             : private:
+      74             :   ros::NodeHandle nh_;
+      75             : 
+      76             :   bool callbacks_enabled_ = true;
+      77             : 
+      78             :   std::string _uav_name_;
+      79             : 
+      80             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      81             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      82             : 
+      83             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> ph_rviz_marker_;
+      84             : 
+      85             :   // | ------------------------ uav state ----------------------- |
+      86             : 
+      87             :   mrs_msgs::UavState uav_state_;
+      88             :   bool               got_uav_state_ = false;
+      89             :   std::mutex         mutex_uav_state_;
+      90             : 
+      91             :   // | ------------------- tracker constraints ------------------ |
+      92             : 
+      93             :   mrs_msgs::DynamicsConstraints constraints_;
+      94             :   std::mutex                    mutex_constraints_;
+      95             : 
+      96             :   // | ---------------- the tracker's inner state --------------- |
+      97             : 
+      98             :   bool is_initialized_  = false;
+      99             :   bool is_active_       = false;
+     100             :   bool first_iteration_ = true;
+     101             : 
+     102             :   double _external_command_timeout_;
+     103             : 
+     104             :   mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand> sh_command_;
+     105             : 
+     106             :   void callbackCommand(const mrs_msgs::SpeedTrackerCommand::ConstPtr msg);
+     107             : 
+     108             :   // stores the post-processed and transformed command
+     109             :   mrs_msgs::SpeedTrackerCommand command_;
+     110             :   std::mutex                    mutex_command_;
+     111             :   ros::Time                     last_command_time_;
+     112             : 
+     113             :   // | ------------------------ profiler ------------------------ |
+     114             : 
+     115             :   mrs_lib::Profiler profiler_;
+     116             :   bool              _profiler_enabled_ = false;
+     117             : };
+     118             : 
+     119             : //}
+     120             : 
+     121             : // | -------------- tracker's interface routines -------------- |
+     122             : 
+     123             : /* //{ initialize() */
+     124             : 
+     125          94 : bool SpeedTracker::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     126             :                               std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     127             : 
+     128          94 :   this->common_handlers_  = common_handlers;
+     129          94 :   this->private_handlers_ = private_handlers;
+     130             : 
+     131          94 :   _uav_name_ = common_handlers->uav_name;
+     132             : 
+     133          94 :   nh_ = nh;
+     134             : 
+     135          94 :   ros::Time::waitForValid();
+     136             : 
+     137             :   // --------------------------------------------------------------
+     138             :   // |                     loading parameters                     |
+     139             :   // --------------------------------------------------------------
+     140             : 
+     141             :   // | ---------------- load parent's parameters ---------------- |
+     142             : 
+     143         188 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     144             : 
+     145          94 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     146             : 
+     147          94 :   if (!param_loader_parent.loadedSuccessfully()) {
+     148           0 :     ROS_ERROR("[SpeedTracker]: Could not load all parameters!");
+     149           0 :     return false;
+     150             :   }
+     151             : 
+     152             :   // | ---------------- load plugin's parameters ---------------- |
+     153             : 
+     154          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/speed_tracker.yaml");
+     155          94 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/speed_tracker.yaml");
+     156             : 
+     157         188 :   const std::string yaml_prefix = "mrs_uav_trackers/speed_tracker/";
+     158             : 
+     159          94 :   private_handlers->param_loader->loadParam(yaml_prefix + "command_timeout", _external_command_timeout_);
+     160             : 
+     161          94 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     162           0 :     ROS_ERROR("[SpeedTracker]: could not load all parameters!");
+     163           0 :     return false;
+     164             :   }
+     165             : 
+     166             :   // | ------------------------ profiler ------------------------ |
+     167             : 
+     168          94 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "SpeedTracker", _profiler_enabled_);
+     169             : 
+     170             :   // | ----------------------- subscribers ---------------------- |
+     171             : 
+     172          94 :   mrs_lib::SubscribeHandlerOptions shopts;
+     173          94 :   shopts.nh              = nh_;
+     174          94 :   shopts.node_name       = "SpeedTracker";
+     175          94 :   shopts.threadsafe      = true;
+     176          94 :   shopts.autostart       = true;
+     177          94 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     178             : 
+     179          94 :   sh_command_ = mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand>(shopts, "command", &SpeedTracker::callbackCommand, this);
+     180             : 
+     181             :   // | ----------------------- publishers ----------------------- |
+     182             : 
+     183          94 :   ph_rviz_marker_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "rviz_marker", 1);
+     184             : 
+     185             :   // | --------------------- finish the init -------------------- |
+     186             : 
+     187          94 :   is_initialized_ = true;
+     188             : 
+     189          94 :   ROS_INFO("[SpeedTracker]: initialized");
+     190             : 
+     191          94 :   return true;
+     192             : }
+     193             : 
+     194             : //}
+     195             : 
+     196             : /* //{ activate() */
+     197             : 
+     198           0 : std::tuple<bool, std::string> SpeedTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     199             : 
+     200           0 :   std::stringstream ss;
+     201             : 
+     202           0 :   if (!got_uav_state_) {
+     203           0 :     ss << "odometry not set";
+     204           0 :     ROS_ERROR_STREAM("[SpeedTracker]: " << ss.str());
+     205           0 :     return std::tuple(false, ss.str());
+     206             :   }
+     207             : 
+     208           0 :   if (!sh_command_.hasMsg()) {
+     209           0 :     ss << "missing command";
+     210           0 :     ROS_ERROR_STREAM("[SpeedTracker]: " << ss.str());
+     211           0 :     return std::tuple(false, ss.str());
+     212             :   }
+     213             : 
+     214           0 :   ros::Time external_command_time = sh_command_.lastMsgTime();
+     215             : 
+     216             :   // timeout the external command
+     217           0 :   if ((ros::Time::now() - external_command_time).toSec() > _external_command_timeout_) {
+     218           0 :     ss << "the command is too old";
+     219           0 :     ROS_ERROR_STREAM("[SpeedTracker]: " << ss.str());
+     220           0 :     return std::tuple(false, ss.str());
+     221             :   }
+     222             : 
+     223           0 :   is_active_ = true;
+     224             : 
+     225           0 :   ss << "activated";
+     226           0 :   ROS_INFO_STREAM("[SpeedTracker]: " << ss.str());
+     227             : 
+     228           0 :   return std::tuple(true, ss.str());
+     229             : }
+     230             : 
+     231             : //}
+     232             : 
+     233             : /* //{ deactivate() */
+     234             : 
+     235          20 : void SpeedTracker::deactivate(void) {
+     236             : 
+     237          20 :   is_active_ = false;
+     238             : 
+     239          20 :   ROS_INFO("[SpeedTracker]: deactivated");
+     240          20 : }
+     241             : 
+     242             : //}
+     243             : 
+     244             : /* //{ resetStatic() */
+     245             : 
+     246           0 : bool SpeedTracker::resetStatic(void) {
+     247             : 
+     248           0 :   return false;
+     249             : }
+     250             : 
+     251             : //}
+     252             : 
+     253             : /* //{ update() */
+     254             : 
+     255      127134 : std::optional<mrs_msgs::TrackerCommand> SpeedTracker::update(const mrs_msgs::UavState &                                          uav_state,
+     256             :                                                              [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput &last_control_output) {
+     257             : 
+     258      381402 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     259      381402 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     260             : 
+     261             :   {
+     262      127134 :     std::scoped_lock lock(mutex_uav_state_);
+     263             : 
+     264      127134 :     uav_state_ = uav_state;
+     265             : 
+     266      127134 :     got_uav_state_ = true;
+     267             :   }
+     268             : 
+     269             :   double uav_heading;
+     270             : 
+     271             :   try {
+     272      127134 :     uav_heading = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+     273             :   }
+     274           0 :   catch (...) {
+     275           0 :     ROS_ERROR_THROTTLE(1.0, "[SpeedTracker]: could not calculate UAV heading");
+     276             : 
+     277           0 :     return {};
+     278             :   }
+     279             : 
+     280             :   // up to this part the update() method is evaluated even when the tracker is not active
+     281      127134 :   if (!is_active_) {
+     282      127134 :     return {};
+     283             :   }
+     284             : 
+     285           0 :   ros::Time external_command_time = sh_command_.lastMsgTime();
+     286             : 
+     287             :   // timeout the external command
+     288           0 :   if (sh_command_.hasMsg() && (ros::Time::now() - external_command_time).toSec() > _external_command_timeout_) {
+     289           0 :     ROS_ERROR("[SpeedTracker]: command timeouted, returning nil");
+     290           0 :     first_iteration_ = true;
+     291           0 :     return {};
+     292             :   }
+     293             : 
+     294           0 :   auto command = mrs_lib::get_mutexed(mutex_command_, command_);
+     295             : 
+     296           0 :   mrs_msgs::TrackerCommand tracker_cmd;
+     297             : 
+     298           0 :   tracker_cmd.header.stamp    = ros::Time::now();
+     299           0 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     300             : 
+     301           0 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     302           0 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     303             : 
+     304           0 :   if (command.use_velocity) {
+     305           0 :     tracker_cmd.velocity.x              = command.velocity.x;
+     306           0 :     tracker_cmd.velocity.y              = command.velocity.y;
+     307           0 :     tracker_cmd.velocity.z              = command.velocity.z;
+     308           0 :     tracker_cmd.use_velocity_horizontal = true;
+     309           0 :     tracker_cmd.use_velocity_vertical   = true;
+     310             :   } else {
+     311           0 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     312           0 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     313           0 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     314           0 :     tracker_cmd.use_velocity_horizontal = false;
+     315           0 :     tracker_cmd.use_velocity_vertical   = false;
+     316             :   }
+     317             : 
+     318           0 :   if (command.use_z) {
+     319           0 :     tracker_cmd.position.z            = command.z;
+     320           0 :     tracker_cmd.use_position_vertical = true;
+     321             :   } else {
+     322           0 :     tracker_cmd.position.z            = uav_state.pose.position.z;
+     323           0 :     tracker_cmd.use_position_vertical = false;
+     324             :   }
+     325             : 
+     326           0 :   if (command.use_acceleration) {
+     327           0 :     tracker_cmd.acceleration.x   = command.acceleration.x;
+     328           0 :     tracker_cmd.acceleration.y   = command.acceleration.y;
+     329           0 :     tracker_cmd.acceleration.z   = command.acceleration.z;
+     330           0 :     tracker_cmd.use_acceleration = true;
+     331           0 :   } else if (command.use_force) {
+     332           0 :     tracker_cmd.acceleration.x   = command.force.x / last_control_output.diagnostics.total_mass;
+     333           0 :     tracker_cmd.acceleration.y   = command.force.y / last_control_output.diagnostics.total_mass;
+     334           0 :     tracker_cmd.acceleration.z   = command.force.z / last_control_output.diagnostics.total_mass;
+     335           0 :     tracker_cmd.use_acceleration = true;
+     336             :   } else {
+     337           0 :     tracker_cmd.acceleration.x   = 0;
+     338           0 :     tracker_cmd.acceleration.y   = 0;
+     339           0 :     tracker_cmd.acceleration.z   = 0;
+     340           0 :     tracker_cmd.use_acceleration = false;
+     341             :   }
+     342             : 
+     343           0 :   if (command.use_heading) {
+     344           0 :     tracker_cmd.heading     = command.heading;
+     345           0 :     tracker_cmd.use_heading = true;
+     346             :   } else {
+     347           0 :     tracker_cmd.heading     = uav_heading;
+     348           0 :     tracker_cmd.use_heading = false;
+     349             :   }
+     350             : 
+     351           0 :   if (command.use_heading_rate) {
+     352           0 :     tracker_cmd.heading_rate     = command.heading_rate;
+     353           0 :     tracker_cmd.use_heading_rate = true;
+     354             :   } else {
+     355           0 :     tracker_cmd.heading_rate     = uav_state.velocity.angular.z;
+     356           0 :     tracker_cmd.use_heading_rate = false;
+     357             :   }
+     358             : 
+     359           0 :   return {tracker_cmd};
+     360             : }
+     361             : 
+     362             : //}
+     363             : 
+     364             : /* //{ getStatus() */
+     365             : 
+     366           0 : const mrs_msgs::TrackerStatus SpeedTracker::getStatus() {
+     367             : 
+     368           0 :   mrs_msgs::TrackerStatus tracker_status;
+     369             : 
+     370           0 :   tracker_status.active            = is_active_;
+     371           0 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     372             : 
+     373           0 :   return tracker_status;
+     374             : }
+     375             : 
+     376             : //}
+     377             : 
+     378             : /* //{ enableCallbacks() */
+     379             : 
+     380         297 : const std_srvs::SetBoolResponse::ConstPtr SpeedTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     381             : 
+     382         594 :   std_srvs::SetBoolResponse res;
+     383         594 :   std::stringstream         ss;
+     384             : 
+     385         297 :   if (cmd->data != callbacks_enabled_) {
+     386             : 
+     387          20 :     callbacks_enabled_ = cmd->data;
+     388             : 
+     389          20 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     390          20 :     ROS_INFO_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     391             : 
+     392             :   } else {
+     393             : 
+     394         277 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     395         277 :     ROS_WARN_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     396             :   }
+     397             : 
+     398         297 :   res.message = ss.str();
+     399         297 :   res.success = true;
+     400             : 
+     401         594 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     402             : }
+     403             : 
+     404             : //}
+     405             : 
+     406             : /* switchOdometrySource() //{ */
+     407             : 
+     408           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     409             : 
+     410           0 :   return std_srvs::TriggerResponse::Ptr();
+     411             : }
+     412             : 
+     413             : //}
+     414             : 
+     415             : /* //{ hover() */
+     416             : 
+     417           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     418             : 
+     419           0 :   return std_srvs::TriggerResponse::Ptr();
+     420             : }
+     421             : 
+     422             : //}
+     423             : 
+     424             : /* //{ startTrajectoryTracking() */
+     425             : 
+     426           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     427           0 :   return std_srvs::TriggerResponse::Ptr();
+     428             : }
+     429             : 
+     430             : //}
+     431             : 
+     432             : /* //{ stopTrajectoryTracking() */
+     433             : 
+     434           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     435           0 :   return std_srvs::TriggerResponse::Ptr();
+     436             : }
+     437             : 
+     438             : //}
+     439             : 
+     440             : /* //{ resumeTrajectoryTracking() */
+     441             : 
+     442           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     443           0 :   return std_srvs::TriggerResponse::Ptr();
+     444             : }
+     445             : 
+     446             : //}
+     447             : 
+     448             : /* //{ gotoTrajectoryStart() */
+     449             : 
+     450           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     451           0 :   return std_srvs::TriggerResponse::Ptr();
+     452             : }
+     453             : 
+     454             : //}
+     455             : 
+     456             : /* //{ setConstraints() */
+     457             : 
+     458         325 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr SpeedTracker::setConstraints([
+     459             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     460             : 
+     461             :   {
+     462         325 :     std::scoped_lock lock(mutex_constraints_);
+     463             : 
+     464         325 :     constraints_ = cmd->constraints;
+     465             :   }
+     466             : 
+     467         650 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     468             : 
+     469         325 :   res.success = true;
+     470         325 :   res.message = "constraints updated";
+     471             : 
+     472         650 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     473             : }
+     474             : 
+     475             : //}
+     476             : 
+     477             : /* //{ setReference() */
+     478             : 
+     479           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr SpeedTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     480             : 
+     481           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     482             : }
+     483             : 
+     484             : //}
+     485             : 
+     486             : /* //{ setVelocityReference() */
+     487             : 
+     488           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr SpeedTracker::setVelocityReference([
+     489             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     490           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     491             : }
+     492             : 
+     493             : //}
+     494             : 
+     495             : /* //{ setTrajectoryReference() */
+     496             : 
+     497           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr SpeedTracker::setTrajectoryReference([
+     498             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     499           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     500             : }
+     501             : 
+     502             : //}
+     503             : 
+     504             : // | --------------------- custom methods --------------------- |
+     505             : 
+     506             : /* callbackCommand() //{ */
+     507             : 
+     508           0 : void SpeedTracker::callbackCommand(const mrs_msgs::SpeedTrackerCommand::ConstPtr msg) {
+     509             : 
+     510           0 :   if (!is_initialized_)
+     511           0 :     return;
+     512             : 
+     513           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackCommand");
+     514           0 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::callbackCommand", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     515             : 
+     516           0 :   mrs_msgs::SpeedTrackerCommandConstPtr external_command = msg;
+     517             : 
+     518             :   double dt;
+     519           0 :   if (first_iteration_) {
+     520             : 
+     521           0 :     last_command_time_ = ros::Time::now();
+     522           0 :     first_iteration_   = false;
+     523             : 
+     524             :     {
+     525           0 :       std::scoped_lock lock(mutex_command_);
+     526             : 
+     527           0 :       command_ = *external_command;
+     528             :     }
+     529             : 
+     530           0 :     return;
+     531             :   } else {
+     532           0 :     dt                 = (ros::Time::now() - last_command_time_).toSec();
+     533           0 :     last_command_time_ = ros::Time::now();
+     534             : 
+     535           0 :     if (dt <= 1e-4) {
+     536           0 :       ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: the command dt is %.5f, returning", dt);
+     537           0 :       return;
+     538             :     }
+     539             :   }
+     540             : 
+     541           0 :   mrs_msgs::SpeedTrackerCommand transformed_command = *external_command;
+     542             : 
+     543           0 :   auto old_command = mrs_lib::get_mutexed(mutex_command_, command_);
+     544           0 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     545           0 :   auto uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     546             : 
+     547             :   double uav_heading;
+     548             : 
+     549             :   try {
+     550           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+     551             :   }
+     552           0 :   catch (...) {
+     553           0 :     ROS_ERROR_THROTTLE(1.0, "[SpeedTracker]: could not calculate UAV heading");
+     554           0 :     return;
+     555             :   }
+     556             : 
+     557             :   // transform the command
+     558             : 
+     559             :   // transform velocity
+     560             : 
+     561           0 :   if (transformed_command.use_velocity) {
+     562             : 
+     563           0 :     geometry_msgs::Vector3Stamped vector3;
+     564           0 :     vector3.header = transformed_command.header;
+     565             : 
+     566           0 :     vector3.vector.x = transformed_command.velocity.x;
+     567           0 :     vector3.vector.y = transformed_command.velocity.y;
+     568           0 :     vector3.vector.z = transformed_command.velocity.z;
+     569             : 
+     570           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     571             : 
+     572           0 :     if (ret) {
+     573           0 :       transformed_command.velocity.x = ret.value().vector.x;
+     574           0 :       transformed_command.velocity.y = ret.value().vector.y;
+     575           0 :       transformed_command.velocity.z = ret.value().vector.z;
+     576             :     } else {
+     577           0 :       return;
+     578             :     }
+     579             : 
+     580             :     /* horizontal speed limit //{ */
+     581             : 
+     582             :     {
+     583           0 :       double des_horizontal_speed = sqrt(pow(transformed_command.velocity.x, 2) + pow(transformed_command.velocity.y, 2));
+     584             : 
+     585           0 :       if (des_horizontal_speed > constraints.horizontal_speed) {
+     586             : 
+     587           0 :         double des_speed_heading = atan2(transformed_command.velocity.y, transformed_command.velocity.x);
+     588             : 
+     589           0 :         transformed_command.velocity.x = cos(des_speed_heading) * constraints.horizontal_speed;
+     590           0 :         transformed_command.velocity.y = sin(des_speed_heading) * constraints.horizontal_speed;
+     591             :       }
+     592             :     }
+     593             : 
+     594             :     //}
+     595             : 
+     596             :     /* horizontal speed change rate limit //{ */
+     597             : 
+     598             :     {
+     599             :       Eigen::Vector2d hor_speed_derivative =
+     600           0 :           Eigen::Vector2d(transformed_command.velocity.x - old_command.velocity.x, transformed_command.velocity.y - old_command.velocity.y) / dt;
+     601             : 
+     602             :       // exceeding the maximum acceleration
+     603           0 :       if (hor_speed_derivative.norm() > constraints.horizontal_acceleration) {
+     604             : 
+     605           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting speed change rate");
+     606           0 :         double direction = atan2(hor_speed_derivative[1], hor_speed_derivative[0]);
+     607             : 
+     608           0 :         transformed_command.velocity.x = old_command.velocity.x + cos(direction) * constraints.horizontal_acceleration * dt;
+     609           0 :         transformed_command.velocity.y = old_command.velocity.y + sin(direction) * constraints.horizontal_acceleration * dt;
+     610             :       }
+     611             :     }
+     612             : 
+     613             :     //}
+     614             : 
+     615             :     /* vertical speed limit //{ */
+     616             : 
+     617             :     {
+     618             :       // if ascending
+     619           0 :       if (transformed_command.velocity.z > constraints.vertical_ascending_speed) {
+     620             : 
+     621           0 :         transformed_command.velocity.z = constraints.vertical_ascending_speed;
+     622             :       }
+     623             : 
+     624             :       // if descending
+     625           0 :       if (transformed_command.velocity.z < -constraints.vertical_descending_speed) {
+     626             : 
+     627           0 :         transformed_command.velocity.z = -constraints.vertical_descending_speed;
+     628             :       }
+     629             :     }
+     630             : 
+     631             :     //}
+     632             : 
+     633             :     /* vertical speed change rate //{ */
+     634             : 
+     635             :     {
+     636             : 
+     637           0 :       double vert_speed_derivative = (transformed_command.velocity.z - old_command.velocity.z) / dt;
+     638             : 
+     639           0 :       if (vert_speed_derivative > constraints.vertical_ascending_acceleration) {
+     640             : 
+     641           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical ascending speed change rate");
+     642           0 :         transformed_command.velocity.z = old_command.velocity.z + constraints.vertical_ascending_acceleration * dt;
+     643             : 
+     644           0 :       } else if (vert_speed_derivative < -constraints.vertical_descending_acceleration) {
+     645             : 
+     646           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical descending speed change rate");
+     647           0 :         transformed_command.velocity.z = old_command.velocity.z - constraints.vertical_descending_acceleration * dt;
+     648             :       }
+     649             :     }
+     650             : 
+     651             :     //}
+     652             :   }
+     653             : 
+     654             :   /* transform and constrain heading //{ */
+     655             : 
+     656           0 :   if (transformed_command.use_heading) {
+     657             : 
+     658           0 :     mrs_msgs::ReferenceStamped temp_ref;
+     659           0 :     temp_ref.header = transformed_command.header;
+     660             : 
+     661           0 :     temp_ref.reference.heading = transformed_command.heading;
+     662             : 
+     663           0 :     auto ret = common_handlers_->transformer->transformSingle(temp_ref, "");
+     664             : 
+     665           0 :     if (ret) {
+     666             : 
+     667             :       // calculate the produced heading rate
+     668           0 :       double des_hdg_rate = sradians::diff(ret.value().reference.heading, old_command.heading) / dt;
+     669             : 
+     670             :       // saturate the change in the desired heading
+     671           0 :       if (des_hdg_rate > constraints.heading_speed) {
+     672             : 
+     673           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting change of the desired heading using constraints");
+     674           0 :         transformed_command.heading = old_command.heading + constraints.heading_speed * dt;
+     675             : 
+     676           0 :       } else if (des_hdg_rate < -constraints.heading_speed) {
+     677             : 
+     678           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting change of the desired heading using constraints");
+     679           0 :         transformed_command.heading = old_command.heading - constraints.heading_speed * dt;
+     680             : 
+     681             :       } else {
+     682             : 
+     683           0 :         transformed_command.heading = ret.value().reference.heading;
+     684             :       }
+     685             : 
+     686             :     } else {
+     687           0 :       return;
+     688             :     }
+     689             :   } else {
+     690           0 :     transformed_command.use_heading = false;
+     691           0 :     transformed_command.heading     = uav_heading;
+     692             :   }
+     693             : 
+     694             :   //}
+     695             : 
+     696           0 :   if (transformed_command.use_heading_rate) {
+     697             : 
+     698           0 :     if (transformed_command.heading_rate > constraints.heading_speed) {
+     699           0 :       transformed_command.heading_rate = constraints.heading_speed;
+     700           0 :     } else if (transformed_command.heading_rate < -constraints.heading_speed) {
+     701           0 :       transformed_command.heading_rate = -constraints.heading_speed;
+     702             :     }
+     703             :   }
+     704             : 
+     705           0 :   if (transformed_command.use_acceleration) {
+     706             : 
+     707           0 :     geometry_msgs::Vector3Stamped vector3;
+     708           0 :     vector3.header = transformed_command.header;
+     709             : 
+     710           0 :     vector3.vector.x = transformed_command.acceleration.x;
+     711           0 :     vector3.vector.y = transformed_command.acceleration.y;
+     712           0 :     vector3.vector.z = transformed_command.acceleration.z;
+     713             : 
+     714           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     715             : 
+     716           0 :     if (ret) {
+     717           0 :       transformed_command.acceleration.x = ret.value().vector.x;
+     718           0 :       transformed_command.acceleration.y = ret.value().vector.y;
+     719           0 :       transformed_command.acceleration.z = ret.value().vector.z;
+     720             :     } else {
+     721           0 :       return;
+     722             :     }
+     723             : 
+     724             :     /* horizontal acceleration limit //{ */
+     725             : 
+     726             :     {
+     727           0 :       double des_horizontal_acceleration = sqrt(pow(transformed_command.acceleration.x, 2) + pow(transformed_command.acceleration.y, 2));
+     728             : 
+     729           0 :       if (des_horizontal_acceleration > constraints.horizontal_acceleration) {
+     730             : 
+     731           0 :         double des_acc_heading = atan2(transformed_command.acceleration.y, transformed_command.acceleration.x);
+     732             : 
+     733           0 :         transformed_command.acceleration.x = cos(des_acc_heading) * constraints.horizontal_acceleration;
+     734           0 :         transformed_command.acceleration.y = sin(des_acc_heading) * constraints.horizontal_acceleration;
+     735             :       }
+     736             :     }
+     737             : 
+     738             :     //}
+     739             : 
+     740             :     /* horizontal acceleration change rate limit //{ */
+     741             : 
+     742             :     {
+     743             :       Eigen::Vector2d hor_acc_derivative =
+     744           0 :           Eigen::Vector2d(transformed_command.acceleration.x - old_command.acceleration.x, transformed_command.acceleration.y - old_command.acceleration.y) /
+     745           0 :           (dt);
+     746             : 
+     747             :       // exceeding the maximum acceleration
+     748           0 :       if (hor_acc_derivative.norm() > constraints.horizontal_jerk) {
+     749             : 
+     750           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting acceleration change rate");
+     751           0 :         double direction = atan2(hor_acc_derivative[1], hor_acc_derivative[0]);
+     752             : 
+     753           0 :         transformed_command.acceleration.x = old_command.acceleration.x + cos(direction) * constraints.horizontal_jerk * dt;
+     754           0 :         transformed_command.acceleration.y = old_command.acceleration.y + sin(direction) * constraints.horizontal_jerk * dt;
+     755             :       }
+     756             :     }
+     757             : 
+     758             :     //}
+     759             : 
+     760             :     /* vertical acceleration limit //{ */
+     761             : 
+     762             :     {
+     763             :       // if ascending
+     764           0 :       if (transformed_command.acceleration.z > constraints.vertical_ascending_acceleration) {
+     765             : 
+     766           0 :         transformed_command.acceleration.z = constraints.vertical_ascending_acceleration;
+     767             :       }
+     768             : 
+     769             :       // if descending
+     770           0 :       if (transformed_command.acceleration.z < -constraints.vertical_descending_acceleration) {
+     771             : 
+     772           0 :         transformed_command.acceleration.z = -constraints.vertical_descending_acceleration;
+     773             :       }
+     774             :     }
+     775             : 
+     776             :     //}
+     777             : 
+     778             :     /* vertical acceleration change rate //{ */
+     779             : 
+     780             :     {
+     781             : 
+     782           0 :       double vert_acc_derivative = (transformed_command.acceleration.z - old_command.acceleration.z) / dt;
+     783             : 
+     784           0 :       if (vert_acc_derivative > constraints.vertical_ascending_jerk) {
+     785             : 
+     786           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical ascending acceleration change rate");
+     787           0 :         transformed_command.acceleration.z = old_command.acceleration.z + constraints.vertical_ascending_jerk * dt;
+     788             : 
+     789           0 :       } else if (vert_acc_derivative < -constraints.vertical_descending_jerk) {
+     790             : 
+     791           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical descending acceleration change rate");
+     792           0 :         transformed_command.acceleration.z = old_command.acceleration.z - constraints.vertical_descending_jerk * dt;
+     793             :       }
+     794             :     }
+     795             : 
+     796             :     //}
+     797             :   }
+     798             : 
+     799             :   // transform force
+     800             : 
+     801           0 :   if (transformed_command.use_force) {
+     802             : 
+     803           0 :     geometry_msgs::Vector3Stamped vector3;
+     804           0 :     vector3.header = transformed_command.header;
+     805             : 
+     806           0 :     vector3.vector.x = transformed_command.force.x;
+     807           0 :     vector3.vector.y = transformed_command.force.y;
+     808           0 :     vector3.vector.z = transformed_command.force.z;
+     809             : 
+     810           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     811             : 
+     812           0 :     if (ret) {
+     813           0 :       transformed_command.force.x = vector3.vector.x;
+     814           0 :       transformed_command.force.y = vector3.vector.y;
+     815           0 :       transformed_command.force.z = vector3.vector.z;
+     816             :     } else {
+     817           0 :       return;
+     818             :     }
+     819             :   }
+     820             : 
+     821             :   // check the feasibility of the z
+     822             :   {
+     823           0 :     double z_derivative = (transformed_command.z - old_command.z) / dt;
+     824             : 
+     825           0 :     if (z_derivative > constraints.vertical_ascending_speed) {
+     826             : 
+     827           0 :       transformed_command.z = old_command.z + constraints.vertical_ascending_speed * dt;
+     828             : 
+     829           0 :     } else if (z_derivative < -constraints.vertical_ascending_speed) {
+     830             : 
+     831           0 :       transformed_command.z = old_command.z - constraints.vertical_descending_speed * dt;
+     832             :     }
+     833             : 
+     834             :     // saturate the desired z using the safety area
+     835           0 :     if (common_handlers_->safety_area.use_safety_area) {
+     836             : 
+     837           0 :       if (transformed_command.z > common_handlers_->safety_area.getMaxZ("")) {
+     838             : 
+     839           0 :         transformed_command.z = common_handlers_->safety_area.getMaxZ("");
+     840             : 
+     841           0 :       } else if (transformed_command.z < common_handlers_->safety_area.getMinZ("")) {
+     842             : 
+     843           0 :         transformed_command.z = common_handlers_->safety_area.getMinZ("");
+     844             :       }
+     845             :     }
+     846             :   }
+     847             : 
+     848             :   // if not active, nullify the desired speeds and accelerations
+     849             :   // this will produce a rumpum (using the constraints) after the activation
+     850           0 :   if (!is_active_) {
+     851             : 
+     852           0 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     853             : 
+     854           0 :     transformed_command.velocity.x = 0;
+     855           0 :     transformed_command.velocity.y = 0;
+     856           0 :     transformed_command.velocity.z = 0;
+     857             : 
+     858           0 :     transformed_command.acceleration.x = 0;
+     859           0 :     transformed_command.acceleration.y = 0;
+     860           0 :     transformed_command.acceleration.z = 0;
+     861             : 
+     862             :     try {
+     863           0 :       transformed_command.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     864             :     }
+     865           0 :     catch (...) {
+     866           0 :       return;
+     867             :     }
+     868             : 
+     869           0 :     transformed_command.z = uav_state_.pose.position.z;
+     870             :   }
+     871             : 
+     872             :   {
+     873           0 :     std::scoped_lock lock(mutex_command_);
+     874             : 
+     875           0 :     command_ = transformed_command;
+     876             :   }
+     877             : 
+     878           0 :   if (!is_active_) {
+     879           0 :     ROS_INFO_ONCE("[SpeedTracker]: getting command");
+     880             :   } else {
+     881           0 :     ROS_INFO_THROTTLE(5.0, "[SpeedTracker]: getting command");
+     882             :   }
+     883             : 
+     884             :   // --------------------------------------------------------------
+     885             :   // |                     publish rviz markers                   |
+     886             :   // --------------------------------------------------------------
+     887             : 
+     888           0 :   visualization_msgs::MarkerArray msg_out;
+     889             : 
+     890           0 :   double id = 0;
+     891             : 
+     892           0 :   geometry_msgs::Point point;
+     893             : 
+     894             :   /* desired speed //{ */
+     895             : 
+     896           0 :   if (transformed_command.use_velocity) {
+     897             : 
+     898           0 :     std::scoped_lock lock(mutex_uav_state_);
+     899             : 
+     900           0 :     visualization_msgs::Marker marker;
+     901             : 
+     902           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+     903           0 :     marker.header.stamp    = ros::Time::now();
+     904           0 :     marker.ns              = "speed_tracker";
+     905           0 :     marker.id              = id++;
+     906           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+     907           0 :     marker.action          = visualization_msgs::Marker::ADD;
+     908             : 
+     909             :     /* position //{ */
+     910             : 
+     911           0 :     marker.pose.position.x = 0.0;
+     912           0 :     marker.pose.position.y = 0.0;
+     913           0 :     marker.pose.position.z = 0.0;
+     914             : 
+     915             :     //}
+     916             : 
+     917             :     /* orientation //{ */
+     918             : 
+     919           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     920             : 
+     921             :     //}
+     922             : 
+     923             :     /* origin //{ */
+     924           0 :     point.x = uav_state_.pose.position.x;
+     925           0 :     point.y = uav_state_.pose.position.y;
+     926           0 :     point.z = uav_state_.pose.position.z;
+     927             : 
+     928           0 :     marker.points.push_back(point);
+     929             : 
+     930             :     //}
+     931             : 
+     932             :     /* tip //{ */
+     933             : 
+     934           0 :     point.x = uav_state_.pose.position.x + transformed_command.velocity.x;
+     935           0 :     point.y = uav_state_.pose.position.y + transformed_command.velocity.y;
+     936           0 :     point.z = uav_state_.pose.position.z + transformed_command.velocity.z;
+     937             : 
+     938           0 :     marker.points.push_back(point);
+     939             : 
+     940             :     //}
+     941             : 
+     942           0 :     marker.scale.x = 0.05;
+     943           0 :     marker.scale.y = 0.05;
+     944           0 :     marker.scale.z = 0.05;
+     945             : 
+     946           0 :     marker.color.a = 0.5;
+     947           0 :     marker.color.r = 0.0;
+     948           0 :     marker.color.g = 1.0;
+     949           0 :     marker.color.b = 0.0;
+     950             : 
+     951           0 :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+     952             : 
+     953           0 :     msg_out.markers.push_back(marker);
+     954             :   }
+     955             : 
+     956             :   //}
+     957             : 
+     958             :   /* desired acceleration //{ */
+     959           0 :   if (transformed_command.use_acceleration) {
+     960             : 
+     961           0 :     std::scoped_lock lock(mutex_uav_state_);
+     962             : 
+     963           0 :     visualization_msgs::Marker marker;
+     964             : 
+     965           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+     966           0 :     marker.header.stamp    = ros::Time::now();
+     967           0 :     marker.ns              = "speed_tracker";
+     968           0 :     marker.id              = id++;
+     969           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+     970           0 :     marker.action          = visualization_msgs::Marker::ADD;
+     971             : 
+     972             :     /* position //{ */
+     973             : 
+     974           0 :     marker.pose.position.x = 0.0;
+     975           0 :     marker.pose.position.y = 0.0;
+     976           0 :     marker.pose.position.z = 0.0;
+     977             : 
+     978             :     //}
+     979             : 
+     980             :     /* orientation //{ */
+     981             : 
+     982           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     983             : 
+     984             :     //}
+     985             : 
+     986             :     /* origin //{ */
+     987           0 :     point.x = uav_state_.pose.position.x;
+     988           0 :     point.y = uav_state_.pose.position.y;
+     989           0 :     point.z = uav_state_.pose.position.z;
+     990             : 
+     991           0 :     marker.points.push_back(point);
+     992             : 
+     993             :     //}
+     994             : 
+     995             :     /* tip //{ */
+     996             : 
+     997           0 :     point.x = uav_state_.pose.position.x + transformed_command.acceleration.x;
+     998           0 :     point.y = uav_state_.pose.position.y + transformed_command.acceleration.y;
+     999           0 :     point.z = uav_state_.pose.position.z + transformed_command.acceleration.z;
+    1000             : 
+    1001           0 :     marker.points.push_back(point);
+    1002             : 
+    1003             :     //}
+    1004             : 
+    1005           0 :     marker.scale.x = 0.05;
+    1006           0 :     marker.scale.y = 0.05;
+    1007           0 :     marker.scale.z = 0.05;
+    1008             : 
+    1009           0 :     marker.color.a = 0.5;
+    1010           0 :     marker.color.r = 1.0;
+    1011           0 :     marker.color.g = 0.0;
+    1012           0 :     marker.color.b = 0.0;
+    1013             : 
+    1014           0 :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    1015             : 
+    1016           0 :     msg_out.markers.push_back(marker);
+    1017             :   }
+    1018             : 
+    1019             :   //}
+    1020             : 
+    1021             :   /* desired force //{ */
+    1022           0 :   if (transformed_command.use_force) {
+    1023             : 
+    1024           0 :     std::scoped_lock lock(mutex_uav_state_);
+    1025             : 
+    1026           0 :     visualization_msgs::Marker marker;
+    1027             : 
+    1028           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+    1029           0 :     marker.header.stamp    = ros::Time::now();
+    1030           0 :     marker.ns              = "speed_tracker";
+    1031           0 :     marker.id              = id++;
+    1032           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+    1033           0 :     marker.action          = visualization_msgs::Marker::ADD;
+    1034             : 
+    1035             :     /* position //{ */
+    1036             : 
+    1037           0 :     marker.pose.position.x = 0.0;
+    1038           0 :     marker.pose.position.y = 0.0;
+    1039           0 :     marker.pose.position.z = 0.0;
+    1040             : 
+    1041             :     //}
+    1042             : 
+    1043             :     /* orientation //{ */
+    1044             : 
+    1045           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1046             : 
+    1047             :     //}
+    1048             : 
+    1049             :     /* origin //{ */
+    1050           0 :     point.x = uav_state_.pose.position.x;
+    1051           0 :     point.y = uav_state_.pose.position.y;
+    1052           0 :     point.z = uav_state_.pose.position.z;
+    1053             : 
+    1054           0 :     marker.points.push_back(point);
+    1055             : 
+    1056             :     //}
+    1057             : 
+    1058             :     /* tip //{ */
+    1059             : 
+    1060           0 :     point.x = uav_state_.pose.position.x + transformed_command.force.x;
+    1061           0 :     point.y = uav_state_.pose.position.y + transformed_command.force.y;
+    1062           0 :     point.z = uav_state_.pose.position.z + transformed_command.force.z;
+    1063             : 
+    1064           0 :     marker.points.push_back(point);
+    1065             : 
+    1066             :     //}
+    1067             : 
+    1068           0 :     marker.scale.x = 0.05;
+    1069           0 :     marker.scale.y = 0.05;
+    1070           0 :     marker.scale.z = 0.05;
+    1071             : 
+    1072           0 :     marker.color.a = 0.5;
+    1073           0 :     marker.color.r = 0.0;
+    1074           0 :     marker.color.g = 0.0;
+    1075           0 :     marker.color.b = 1.0;
+    1076             : 
+    1077           0 :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    1078             : 
+    1079           0 :     msg_out.markers.push_back(marker);
+    1080             :   }
+    1081             : 
+    1082             :   //}
+    1083             : 
+    1084           0 :   ph_rviz_marker_.publish(msg_out);
+    1085             : }
+    1086             : 
+    1087             : //}
+    1088             : 
+    1089             : }  // namespace speed_tracker
+    1090             : 
+    1091             : }  // namespace mrs_uav_trackers
+    1092             : 
+    1093             : #include <pluginlib/class_list_macros.h>
+    1094          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::speed_tracker::SpeedTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..b1299a71fe --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html @@ -0,0 +1,294 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.png b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..76f0064d0030a9ff28cfd278c8a71f07be11d779 GIT binary patch literal 3011 zcmV;!3q16RP)Wz0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp$C1x zQk)MaXFbBZj8<|jnq#C^6?(El&+TBr1`4}g=H|HZhSBSLfMvYz{@c~`W z)*~$oDipmayS10Z-b)7L8Zid5CH^ep+YNr8+C+kj9tQb ziW6m%1i1IW0^Zby^>kyd3q=&kHh0-5VX;h%TmTnQoP(8*k4ML?3bWAMmPI=)!qbX)ID-^TPWO2aSbp}$$m$_e!v+C00)xN*)-${GKpYVUC zue5zrjo&q-@%0G56)gci0`P$VnaxqWC!&}1WE5YVVI)rSNUKFoaY92K9N5v%qM5=p z4BQm}nH1R#PMWUp4C9Y{&;UJVuAaW%Ynhx*bS4Q!oV9xnT-PUT+x~ZZn%5uOR@W@> z_1|W#w`px5Jn(mzu-XJbfvs)Caj%bpmIn$bQVmeQi+H3Q`=E4L@5v|3Gv0$34w&`$ zDGH>JrEHO+;^FsiH0C>D>_1-f7dhiZiWUfcl>k00kWH}-{LbgtlnLpWp==o2ITM~+ zUwf%rT=w$l7Z|e=kF{i_Gn#qHPgAUTR|GP$Eem9~A=_8ZwX`UL;QeamSvGV0op^zH zkKyry)o3oExCJ&F6rY=MUDsxteCFCeVQeP`0mj}-L;$MKTtMoo;8E%gPP&(sI(0*n zx+LV0+qK{6b|IM@ltAGo9|#+;g;)6g)>iYh>nQw?uR)R5u^p}^vlay`v}~}?$zqRJ zBTC8xnQ9d1aa^z;S)IWry`IWPefk`b0kG$5cGm+GvxA9Vq&fzHm1!Y^1&tqgK8 zMN6{^WX2SM``mi>lxT{~b=ntmmCKiUwTppvq)76fkV?7w&@=onf*@t16azS-g0$>s z8etAO``wq>?b5vvv2K=^Sy6wyGnSu1OikyINgD>`@aFNssQ#Ie2bd5`z3 zPJn#Y&hyib^|e`Z zaZQ52j{?cr###mpWVtZeJTV^F5QemHRTy0E@xVhE3_99zD|&)F8ZZDG!r)`y3nRBo z?79J@#=h^G(eL^L0bosrYXzw9;c3^f`}g`|C=J!OT`p9{(JPxx>E2c{4JIzvm%4g- zxUD-VT0+jxs$B;^!-s;P>kXk$059b|Y7Ugw{Cq(zf2Y69-3A`DfZ7QSe%H3N1}b)F z#iFCY)+pPFDBd5AU{tiNW%-I$-gRmf|2c8j`w92C?Gl0pggN+oB&xrMSAj6V12!f$#rEZ@Wb zupSjc#Yc}iiu~wNC!YTIdQ>`wfUPyKBq`>PkIF|Ll1o)5&jFOn8u_9k9(kaEgOcGw z(?vf$C(c`0a{vZi5(J6DGF{sywoViPA*<)qyLuAtmgZofoda z#!=^Ix^@(|mixYdyl~GSh5Vr)*jV(xM}e}zU{;x}dF{BwavWA@oacll1Xi@Se2t|L zMj=Z-0?Vie`z!#!vl@waibBeUnJtoM03#;=SZp%`oQ%geo z9`0@)o~5YG;+kbCrCsbB-B}8?kd&OpS*tPJ2W?p(Wp|xk5ve$?rZDEO%?d-li6LYZ zD&LKQESu5QP4}{yeB2M%IZH=Nd7-nn>#{`IL$R^QHMSfkpvYe2@+f9*;dX{FTZ6Di zMJV&^$#EBd-`sCY`?Ys;6A&VH51$FtbfazK+Hgq_&6O&7aD& z2XkLz+7zRQzi|&~fWlK^QBVT-6ZLTF60{yrz~ir}hvW6@A;3%3!(G~Z{=`dXJ)Cg3 z{*~raty)N2#rN-?tb8PNJ;x&8?_cf-C}#e+oc78S^>8$6Sy9uJCh|beMmN)YrtZRd zJ`y%**O|~|o#J>e)+w;Ud2)kWZ_Ie{M8H2cW;8?ij}*?ac&h3Yf#Po}oc}(lFX-XR zc_$xTI7f`KiMP%h#|5_N6g}P}DxA|1m)mu=fi**wMXtEyM?p~ znYPaSWPhmw$FHx=4*&%`{*v`|3{z$GS{?Yl_4Sxh7$2HcIi1Vxn%dFuZerZmd-RkQ z(Q{#m!nqXTBa`r5gS|;8()$mP&mlYxs?NB*qa|!6A$v&F{ke32bk|adJ+c0iR|G*R zk3%=(>pW_w3L`=a*mVrQh3knB=)Xv6o>NfpxdR@@KSPg9g%d~Y4*dsxpGXy+#Iq-d z0DE~PXbu4~d1T)ZSzBA0uJPmJ%Df@+&cb;mPNY|s2kbzKG63rfkvm*HOdj*AdzB}g zYD+DMJ^4WJ3dr-vuBV5>btb$XHb&|*U(uT$HW~L7y~U>ec{khj{2EmZ%Lph&16H3E z&!p(THbL!*_|R)wkg^WNK$ZgbP^_%lA<%%El@vv%xd24B_cLIF$K;u=&`Jf51_kyU ze*#MY6(^eth4Y@*tOVpsJ?g=Zdc!rjoth;*CVpGV`=w?$k4>cNhfSlcm4Qt4l&8Z; zAd}*-$G<2(%o5Whc29K(@DEugsSLA(Q3Nujh7=o*7=zY>cEg(}pS4ckIlor~1!}`EKWSLJ|_(=;3Ais@E z3tQ1*QXkGG_S{?L~`@DEw$lNNr`!UD)|1MY}3 z#|%@Jw>}|%aaV>q&l@x;YMf-hh2m^%D3JbQj`RyU{s9we4cz8Fmj(a;002ovPDHLk FV1i~!w{8Fc literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/src/index-detail-sort-f.html b/mrs_uav_trajectory_generation/src/index-detail-sort-f.html new file mode 100644 index 0000000000..098720468a --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
<unnamed>68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-detail-sort-l.html b/mrs_uav_trajectory_generation/src/index-detail-sort-l.html new file mode 100644 index 0000000000..5fae2ad960 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
<unnamed>68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-detail.html b/mrs_uav_trajectory_generation/src/index-detail.html new file mode 100644 index 0000000000..c259989d8a --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
<unnamed>68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-sort-f.html b/mrs_uav_trajectory_generation/src/index-sort-f.html new file mode 100644 index 0000000000..9500b1fc68 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-sort-l.html b/mrs_uav_trajectory_generation/src/index-sort-l.html new file mode 100644 index 0000000000..fe2b32b68a --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index.html b/mrs_uav_trajectory_generation/src/index.html new file mode 100644 index 0000000000..2c988aeec3 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html new file mode 100644 index 0000000000..682751af3c --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPath(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)2
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)4
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::transformPath(mrs_msgs::Path_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, double const&, bool const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::prepareInitialCondition(ros::Time)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::optimize[abi:cxx11](std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std_msgs::Header_<std::allocator<void> > const&, bool, bool)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::timeLeft()16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::validateTrajectorySpatial(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)17
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::checkNaN(Waypoint_t const&)24
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)94
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()94
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)898
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::distFromSegment(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)4480
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()9760
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)158510
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html new file mode 100644 index 0000000000..4126ef717b --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()94
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)94
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPath(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)4
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::transformPath(mrs_msgs::Path_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::distFromSegment(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)4480
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)158510
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)898
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)2
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, double const&, bool const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::prepareInitialCondition(ros::Time)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::validateTrajectorySpatial(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)17
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()94
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::checkNaN(Waypoint_t const&)24
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::optimize[abi:cxx11](std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std_msgs::Header_<std::allocator<void> > const&, bool, bool)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()9760
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::timeLeft()16
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.frameset.html new file mode 100644 index 0000000000..492e4b05ac --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html new file mode 100644 index 0000000000..e385207763 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html @@ -0,0 +1,2458 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-04-25 05:42:57Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : #include <nodelet/nodelet.h>
+       6             : 
+       7             : #include <mrs_msgs/TrajectoryReferenceSrv.h>
+       8             : #include <mrs_msgs/TrajectoryReference.h>
+       9             : 
+      10             : #include <std_srvs/Trigger.h>
+      11             : 
+      12             : #include <geometry_msgs/Vector3Stamped.h>
+      13             : 
+      14             : #include <mrs_msgs/DynamicsConstraints.h>
+      15             : #include <mrs_msgs/Path.h>
+      16             : #include <mrs_msgs/PathSrv.h>
+      17             : #include <mrs_msgs/GetPathSrv.h>
+      18             : #include <mrs_msgs/TrackerCommand.h>
+      19             : #include <mrs_msgs/MpcPredictionFullState.h>
+      20             : #include <mrs_msgs/Reference.h>
+      21             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      22             : #include <mrs_msgs/UavState.h>
+      23             : 
+      24             : #include <eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h>
+      25             : #include <eth_trajectory_generation/trajectory.h>
+      26             : #include <eth_trajectory_generation/trajectory_sampling.h>
+      27             : 
+      28             : #include <mrs_lib/param_loader.h>
+      29             : #include <mrs_lib/geometry/cyclic.h>
+      30             : #include <mrs_lib/geometry/misc.h>
+      31             : #include <mrs_lib/mutex.h>
+      32             : #include <mrs_lib/batch_visualizer.h>
+      33             : #include <mrs_lib/transformer.h>
+      34             : #include <mrs_lib/utils.h>
+      35             : #include <mrs_lib/scope_timer.h>
+      36             : #include <mrs_lib/attitude_converter.h>
+      37             : #include <mrs_lib/publisher_handler.h>
+      38             : #include <mrs_lib/subscribe_handler.h>
+      39             : 
+      40             : #include <dynamic_reconfigure/server.h>
+      41             : #include <mrs_uav_trajectory_generation/drsConfig.h>
+      42             : 
+      43             : #include <future>
+      44             : 
+      45             : //}
+      46             : 
+      47             : /* using //{ */
+      48             : 
+      49             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      50             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      51             : using mat3_t = Eigen::Matrix3Xd;
+      52             : 
+      53             : using radians  = mrs_lib::geometry::radians;
+      54             : using sradians = mrs_lib::geometry::sradians;
+      55             : 
+      56             : //}
+      57             : 
+      58             : /* defines //{ */
+      59             : 
+      60             : #define FUTURIZATION_EXEC_TIME_FACTOR 0.5        // [0, 1]
+      61             : #define FUTURIZATION_FIRST_WAYPOINT_FACTOR 0.50  // [0, 1]
+      62             : #define OVERTIME_SAFETY_FACTOR 0.95              // [0, 1]
+      63             : #define OVERTIME_SAFETY_OFFSET 0.01              // [s]
+      64             : #define NLOPT_EXEC_TIME_FACTOR 0.95              // [0, 1]
+      65             : 
+      66             : typedef struct
+      67             : {
+      68             :   Eigen::Vector4d coords;
+      69             :   bool            stop_at;
+      70             : } Waypoint_t;
+      71             : 
+      72             : //}
+      73             : 
+      74             : namespace mrs_uav_trajectory_generation
+      75             : {
+      76             : 
+      77             : /* class MrsTrajectoryGeneration //{ */
+      78             : class MrsTrajectoryGeneration : public nodelet::Nodelet {
+      79             : 
+      80             : public:
+      81             :   virtual void onInit();
+      82             : 
+      83             : private:
+      84             :   ros::NodeHandle nh_;
+      85             : 
+      86             :   bool is_initialized_ = false;
+      87             : 
+      88             :   // | ----------------------- parameters ----------------------- |
+      89             : 
+      90             :   double _sampling_dt_;
+      91             : 
+      92             :   double _max_trajectory_len_factor_;
+      93             :   double _min_trajectory_len_factor_;
+      94             : 
+      95             :   int _n_attempts_;
+      96             : 
+      97             :   double _min_waypoint_distance_;
+      98             : 
+      99             :   bool   _fallback_sampling_enabled_;
+     100             :   double _fallback_sampling_speed_factor_;
+     101             :   double _fallback_sampling_accel_factor_;
+     102             :   double _fallback_sampling_stopping_time_;
+     103             :   bool   _fallback_sampling_first_waypoint_additional_stop_;
+     104             : 
+     105             :   std::string _uav_name_;
+     106             : 
+     107             :   bool   _trajectory_max_segment_deviation_enabled_;
+     108             :   double trajectory_max_segment_deviation_;
+     109             :   int    _trajectory_max_segment_deviation_max_iterations_;
+     110             : 
+     111             :   bool   _path_straightener_enabled_;
+     112             :   double _path_straightener_max_deviation_;
+     113             :   double _path_straightener_max_hdg_deviation_;
+     114             : 
+     115             :   // | -------- variable parameters (come with the path) -------- |
+     116             : 
+     117             :   std::string frame_id_;
+     118             :   bool        fly_now_                              = false;
+     119             :   bool        use_heading_                          = false;
+     120             :   bool        stop_at_waypoints_                    = false;
+     121             :   bool        override_constraints_                 = false;
+     122             :   bool        loop_                                 = false;
+     123             :   double      override_max_velocity_horizontal_     = 0.0;
+     124             :   double      override_max_velocity_vertical_       = 0.0;
+     125             :   double      override_max_acceleration_horizontal_ = 0.0;
+     126             :   double      override_max_acceleration_vertical_   = 0.0;
+     127             :   double      override_max_jerk_horizontal_         = 0.0;
+     128             :   double      override_max_jerk_vertical_           = 0.0;
+     129             : 
+     130             :   // | -------------- variable parameters (deduced) ------------- |
+     131             : 
+     132             :   double     max_execution_time_ = 0;
+     133             :   std::mutex mutex_max_execution_time_;
+     134             : 
+     135             :   bool max_deviation_first_segment_;
+     136             : 
+     137             :   std::atomic<bool> dont_prepend_initial_condition_ = false;
+     138             : 
+     139             :   // | -------------------- the transformer  -------------------- |
+     140             : 
+     141             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     142             : 
+     143             :   // | ------------------- scope timer logger ------------------- |
+     144             : 
+     145             :   bool                                       scope_timer_enabled_ = false;
+     146             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     147             : 
+     148             :   // service client for input
+     149             :   bool               callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res);
+     150             :   ros::ServiceServer service_server_path_;
+     151             : 
+     152             :   // service client for returning result to the user
+     153             :   bool               callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res);
+     154             :   ros::ServiceServer service_server_get_path_;
+     155             : 
+     156             :   void                                      callbackPath(const mrs_msgs::Path::ConstPtr msg);
+     157             :   mrs_lib::SubscribeHandler<mrs_msgs::Path> sh_path_;
+     158             : 
+     159             :   mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>      sh_tracker_cmd_;
+     160             :   mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints> sh_constraints_;
+     161             : 
+     162             :   void                                          callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     163             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     164             : 
+     165             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+     166             : 
+     167             :   // service client for publishing trajectory out
+     168             :   ros::ServiceClient service_client_trajectory_reference_;
+     169             : 
+     170             :   // solve the whole problem
+     171             :   std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> optimize(const std::vector<Waypoint_t>& waypoints_in, const std_msgs::Header& waypoints_stamp,
+     172             :                                                                         bool fallback_sampling, const bool relax_heading);
+     173             : 
+     174             :   std::tuple<std::optional<mrs_msgs::TrackerCommand>, bool, int> prepareInitialCondition(const ros::Time path_time);
+     175             : 
+     176             :   // batch vizualizer
+     177             :   mrs_lib::BatchVisualizer bw_original_;
+     178             :   mrs_lib::BatchVisualizer bw_final_;
+     179             : 
+     180             :   // transforming TrackerCommand
+     181             :   std::optional<mrs_msgs::Path> transformPath(const mrs_msgs::Path& path, const std::string& target_frame);
+     182             : 
+     183             :   // | ------------------ trajectory validation ----------------- |
+     184             : 
+     185             :   /**
+     186             :    * @brief validates samples of a trajectory agains a path of waypoints
+     187             :    *
+     188             :    * @param trajectory
+     189             :    * @param segments
+     190             :    *
+     191             :    * @return <success, traj_fail_idx, path_fail_segment>
+     192             :    */
+     193             :   std::tuple<bool, int, std::vector<bool>, double> validateTrajectorySpatial(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+     194             :                                                                              const std::vector<Waypoint_t>&                    waypoints);
+     195             : 
+     196             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectory(const std::vector<Waypoint_t>&                 waypoints,
+     197             :                                                                            const std::optional<mrs_msgs::TrackerCommand>& initial_state,
+     198             :                                                                            const double& sampling_dt, const bool& relax_heading);
+     199             : 
+     200             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectoryFallback(const std::vector<Waypoint_t>& waypoints, const double& sampling_dt,
+     201             :                                                                                    const bool& relax_heading);
+     202             : 
+     203             :   std::future<std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector>> future_trajectory_result_;
+     204             :   std::atomic<bool>                                                      running_async_planning_ = false;
+     205             : 
+     206             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectoryAsync(const std::vector<Waypoint_t>&                 waypoints,
+     207             :                                                                                 const std::optional<mrs_msgs::TrackerCommand>& initial_state,
+     208             :                                                                                 const double& sampling_dt, const bool& relax_heading);
+     209             : 
+     210             :   std::vector<Waypoint_t> preprocessPath(const std::vector<Waypoint_t>& waypoints_in);
+     211             : 
+     212             :   mrs_msgs::TrajectoryReference getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+     213             :                                                        const std::optional<mrs_msgs::TrackerCommand>& initial_condition, const double& sampling_dt);
+     214             : 
+     215             :   Waypoint_t interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff);
+     216             : 
+     217             :   bool checkNaN(const Waypoint_t& a);
+     218             : 
+     219             :   double distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2);
+     220             : 
+     221             :   bool trajectorySrv(const mrs_msgs::TrajectoryReference& msg);
+     222             : 
+     223             :   // | --------------- dynamic reconfigure server --------------- |
+     224             : 
+     225             :   boost::recursive_mutex                           mutex_drs_;
+     226             :   typedef mrs_uav_trajectory_generation::drsConfig DrsParams_t;
+     227             :   typedef dynamic_reconfigure::Server<DrsParams_t> Drs_t;
+     228             :   boost::shared_ptr<Drs_t>                         drs_;
+     229             :   void                                             callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, uint32_t level);
+     230             :   DrsParams_t                                      params_;
+     231             :   std::mutex                                       mutex_params_;
+     232             : 
+     233             :   // | ------------ Republisher for the desired path ------------ |
+     234             : 
+     235             :   mrs_lib::PublisherHandler<mrs_msgs::Path> ph_original_path_;
+     236             : 
+     237             :   // | ------------- measuring the time of execution ------------ |
+     238             : 
+     239             :   ros::Time  start_time_total_;
+     240             :   std::mutex mutex_start_time_total_;
+     241             :   bool       overtime(void);
+     242             :   double     timeLeft(void);
+     243             : };
+     244             : 
+     245             : //}
+     246             : 
+     247             : /* onInit() //{ */
+     248             : 
+     249          94 : void MrsTrajectoryGeneration::onInit() {
+     250             : 
+     251             :   /* obtain node handle */
+     252          94 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     253             : 
+     254             :   /* waits for the ROS to publish clock */
+     255          94 :   ros::Time::waitForValid();
+     256             : 
+     257             :   // | ----------------------- publishers ----------------------- |
+     258             : 
+     259          94 :   ph_original_path_ = mrs_lib::PublisherHandler<mrs_msgs::Path>(nh_, "original_path_out", 1);
+     260             : 
+     261             :   // | ----------------------- subscribers ---------------------- |
+     262             : 
+     263         188 :   mrs_lib::SubscribeHandlerOptions shopts;
+     264          94 :   shopts.nh                 = nh_;
+     265          94 :   shopts.node_name          = "TrajectoryGeneration";
+     266          94 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     267          94 :   shopts.threadsafe         = true;
+     268          94 :   shopts.autostart          = true;
+     269          94 :   shopts.queue_size         = 10;
+     270          94 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     271             : 
+     272          94 :   sh_constraints_          = mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints>(shopts, "constraints_in");
+     273          94 :   sh_tracker_cmd_          = mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>(shopts, "tracker_cmd_in");
+     274          94 :   sh_uav_state_            = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &MrsTrajectoryGeneration::callbackUavState, this);
+     275          94 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diag_in");
+     276             : 
+     277          94 :   sh_path_ = mrs_lib::SubscribeHandler<mrs_msgs::Path>(shopts, "path_in", &MrsTrajectoryGeneration::callbackPath, this);
+     278             : 
+     279             :   // | --------------------- service servers -------------------- |
+     280             : 
+     281          94 :   service_server_path_ = nh_.advertiseService("path_in", &MrsTrajectoryGeneration::callbackPathSrv, this);
+     282             : 
+     283          94 :   service_server_get_path_ = nh_.advertiseService("get_path_in", &MrsTrajectoryGeneration::callbackGetPathSrv, this);
+     284             : 
+     285          94 :   service_client_trajectory_reference_ = nh_.serviceClient<mrs_msgs::TrajectoryReferenceSrv>("trajectory_reference_out");
+     286             : 
+     287             :   // | ----------------------- parameters ----------------------- |
+     288             : 
+     289         188 :   mrs_lib::ParamLoader param_loader(nh_, "MrsTrajectoryGeneration");
+     290             : 
+     291         188 :   std::string custom_config_path;
+     292         188 :   std::string platform_config_path;
+     293             : 
+     294          94 :   param_loader.loadParam("custom_config", custom_config_path);
+     295          94 :   param_loader.loadParam("platform_config", platform_config_path);
+     296             : 
+     297          94 :   if (custom_config_path != "") {
+     298          94 :     param_loader.addYamlFile(custom_config_path);
+     299             :   }
+     300             : 
+     301          94 :   if (platform_config_path != "") {
+     302          94 :     param_loader.addYamlFile(platform_config_path);
+     303             :   }
+     304             : 
+     305          94 :   param_loader.addYamlFileFromParam("private_config");
+     306          94 :   param_loader.addYamlFileFromParam("public_config");
+     307             : 
+     308         188 :   const std::string yaml_prefix = "mrs_uav_trajectory_generation/";
+     309             : 
+     310          94 :   param_loader.loadParam("uav_name", _uav_name_);
+     311             : 
+     312          94 :   param_loader.loadParam(yaml_prefix + "sampling_dt", _sampling_dt_);
+     313             : 
+     314          94 :   param_loader.loadParam(yaml_prefix + "enforce_fallback_solver", params_.enforce_fallback_solver);
+     315             : 
+     316          94 :   param_loader.loadParam(yaml_prefix + "max_trajectory_len_factor", _max_trajectory_len_factor_);
+     317          94 :   param_loader.loadParam(yaml_prefix + "min_trajectory_len_factor", _min_trajectory_len_factor_);
+     318             : 
+     319          94 :   param_loader.loadParam(yaml_prefix + "n_attempts", _n_attempts_);
+     320          94 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/enabled", _fallback_sampling_enabled_);
+     321          94 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/speed_factor", _fallback_sampling_speed_factor_);
+     322          94 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/accel_factor", _fallback_sampling_accel_factor_);
+     323          94 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/stopping_time", _fallback_sampling_stopping_time_);
+     324          94 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/first_waypoint_additional_stop", _fallback_sampling_first_waypoint_additional_stop_);
+     325             : 
+     326          94 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/enabled", _trajectory_max_segment_deviation_enabled_);
+     327          94 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_deviation", params_.max_deviation);
+     328          94 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_iterations", _trajectory_max_segment_deviation_max_iterations_);
+     329             : 
+     330          94 :   param_loader.loadParam(yaml_prefix + "path_straightener/enabled", _path_straightener_enabled_);
+     331          94 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_deviation", _path_straightener_max_deviation_);
+     332          94 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_hdg_deviation", _path_straightener_max_hdg_deviation_);
+     333             : 
+     334          94 :   param_loader.loadParam(yaml_prefix + "min_waypoint_distance", _min_waypoint_distance_);
+     335             : 
+     336             :   // | --------------------- tf transformer --------------------- |
+     337             : 
+     338          94 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "TrajectoryGeneration");
+     339          94 :   transformer_->setDefaultPrefix(_uav_name_);
+     340          94 :   transformer_->retryLookupNewest(true);
+     341             : 
+     342             :   // | ------------------- scope timer logger ------------------- |
+     343             : 
+     344          94 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     345         282 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     346          94 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     347             : 
+     348             :   // | --------------------- service clients -------------------- |
+     349             : 
+     350          94 :   param_loader.loadParam(yaml_prefix + "time_penalty", params_.time_penalty);
+     351          94 :   param_loader.loadParam(yaml_prefix + "soft_constraints_enabled", params_.soft_constraints_enabled);
+     352          94 :   param_loader.loadParam(yaml_prefix + "soft_constraints_weight", params_.soft_constraints_weight);
+     353          94 :   param_loader.loadParam(yaml_prefix + "time_allocation", params_.time_allocation);
+     354          94 :   param_loader.loadParam(yaml_prefix + "equality_constraint_tolerance", params_.equality_constraint_tolerance);
+     355          94 :   param_loader.loadParam(yaml_prefix + "inequality_constraint_tolerance", params_.inequality_constraint_tolerance);
+     356          94 :   param_loader.loadParam(yaml_prefix + "max_iterations", params_.max_iterations);
+     357          94 :   param_loader.loadParam(yaml_prefix + "derivative_to_optimize", params_.derivative_to_optimize);
+     358             : 
+     359          94 :   param_loader.loadParam(yaml_prefix + "max_time", params_.max_execution_time);
+     360             : 
+     361          94 :   max_execution_time_ = params_.max_execution_time;
+     362             : 
+     363          94 :   if (!param_loader.loadedSuccessfully()) {
+     364           0 :     ROS_ERROR("[TrajectoryGeneration]: could not load all parameters!");
+     365           0 :     ros::shutdown();
+     366             :   }
+     367             : 
+     368             :   // | -------------------- batch visualizer -------------------- |
+     369             : 
+     370          94 :   bw_original_ = mrs_lib::BatchVisualizer(nh_, "markers/original", "");
+     371             : 
+     372          94 :   bw_original_.clearBuffers();
+     373          94 :   bw_original_.clearVisuals();
+     374             : 
+     375          94 :   bw_final_ = mrs_lib::BatchVisualizer(nh_, "markers/final", "");
+     376             : 
+     377          94 :   bw_final_.clearBuffers();
+     378          94 :   bw_final_.clearVisuals();
+     379             : 
+     380             :   // | --------------- dynamic reconfigure server --------------- |
+     381             : 
+     382          94 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     383          94 :   drs_->updateConfig(params_);
+     384          94 :   Drs_t::CallbackType f = boost::bind(&MrsTrajectoryGeneration::callbackDrs, this, _1, _2);
+     385          94 :   drs_->setCallback(f);
+     386             : 
+     387             :   // | --------------------- finish the init -------------------- |
+     388             : 
+     389          94 :   ROS_INFO_ONCE("[TrajectoryGeneration]: initialized");
+     390             : 
+     391          94 :   is_initialized_ = true;
+     392          94 : }
+     393             : 
+     394             : //}
+     395             : 
+     396             : // | ---------------------- main routines --------------------- |
+     397             : 
+     398             : /*
+     399             :  * 1. preprocessPath(): preprocessing the incoming path
+     400             :  *    - throughs away too close waypoints
+     401             :  *    - straightness path by neglecting waypoints close to segments
+     402             :  * 2. optimize(): solves the whole problem including
+     403             :  *    - subdivision for satisfying max deviation
+     404             :  * 3. findTrajectory(): solves single instance by the ETH tool
+     405             :  * 4. findTrajectoryFallback(): Baca's sampling for backup solution
+     406             :  * 5. validateTrajectorySpatial(): checks for the spatial soundness of a trajectory vs. the original path
+     407             :  */
+     408             : 
+     409             : /* preprocessPath() //{ */
+     410             : 
+     411          11 : std::vector<Waypoint_t> MrsTrajectoryGeneration::preprocessPath(const std::vector<Waypoint_t>& waypoints_in) {
+     412             : 
+     413          33 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::preprocessPath", scope_timer_logger_, scope_timer_enabled_);
+     414             : 
+     415          11 :   std::vector<Waypoint_t> waypoints;
+     416             : 
+     417          11 :   size_t last_added_idx = 0;  // in "waypoints_in"
+     418             : 
+     419          62 :   for (size_t i = 0; i < waypoints_in.size(); i++) {
+     420             : 
+     421          51 :     double x       = waypoints_in.at(i).coords(0);
+     422          51 :     double y       = waypoints_in.at(i).coords(1);
+     423          51 :     double z       = waypoints_in.at(i).coords(2);
+     424          51 :     double heading = waypoints_in.at(i).coords(3);
+     425             : 
+     426          51 :     bw_original_.addPoint(vec3_t(x, y, z), 1.0, 0.0, 0.0, 1.0);
+     427             : 
+     428          51 :     if (_path_straightener_enabled_ && waypoints_in.size() >= 3 && i > 0 && i < (waypoints_in.size() - 1)) {
+     429             : 
+     430           0 :       vec3_t first(waypoints_in.at(last_added_idx).coords(0), waypoints_in.at(last_added_idx).coords(1), waypoints_in.at(last_added_idx).coords(2));
+     431           0 :       vec3_t last(waypoints_in.at(i + 1).coords(0), waypoints_in.at(i + 1).coords(1), waypoints_in.at(i + 1).coords(2));
+     432             : 
+     433           0 :       double first_hdg = waypoints_in.at(last_added_idx).coords(3);
+     434           0 :       double last_hdg  = waypoints_in.at(i + 1).coords(3);
+     435             : 
+     436           0 :       size_t next_point = last_added_idx + 1;
+     437             : 
+     438           0 :       bool segment_is_ok = true;
+     439             : 
+     440           0 :       for (size_t j = next_point; j < i + 1; j++) {
+     441             : 
+     442           0 :         vec3_t mid(waypoints_in.at(j).coords(0), waypoints_in.at(j).coords(1), waypoints_in.at(j).coords(2));
+     443           0 :         double mid_hdg = waypoints_in.at(j).coords(3);
+     444             : 
+     445           0 :         double dist_from_segment = distFromSegment(mid, first, last);
+     446             : 
+     447           0 :         if (dist_from_segment > _path_straightener_max_deviation_ || fabs(radians::diff(first_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_) ||
+     448           0 :             fabs(radians::diff(last_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_)) {
+     449           0 :           segment_is_ok = false;
+     450           0 :           break;
+     451             :         }
+     452             :       }
+     453             : 
+     454           0 :       if (segment_is_ok) {
+     455           0 :         continue;
+     456             :       }
+     457             :     }
+     458             : 
+     459          51 :     if (i > 0 && i < (waypoints_in.size() - 1)) {
+     460             : 
+     461          29 :       vec3_t first(waypoints_in.at(last_added_idx).coords(0), waypoints_in.at(last_added_idx).coords(1), waypoints_in.at(last_added_idx).coords(2));
+     462          29 :       vec3_t last(waypoints_in.at(i).coords(0), waypoints_in.at(i).coords(1), waypoints_in.at(i).coords(2));
+     463             : 
+     464          29 :       if (mrs_lib::geometry::dist(first, last) < _min_waypoint_distance_) {
+     465           0 :         ROS_INFO("[TrajectoryGeneration]: waypoint #%d too close (< %.3f m) to the previous one (#%d), throwing it away", int(i), _min_waypoint_distance_,
+     466             :                  int(last_added_idx));
+     467           0 :         continue;
+     468             :       }
+     469             :     }
+     470             : 
+     471          51 :     Waypoint_t wp;
+     472          51 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+     473          51 :     wp.stop_at = waypoints_in.at(i).stop_at;
+     474          51 :     waypoints.push_back(wp);
+     475             : 
+     476          51 :     last_added_idx = i;
+     477             :   }
+     478             : 
+     479          22 :   return waypoints;
+     480             : }
+     481             : 
+     482             : //}
+     483             : 
+     484             : /* prepareInitialCondition() //{ */
+     485             : 
+     486          11 : std::tuple<std::optional<mrs_msgs::TrackerCommand>, bool, int> MrsTrajectoryGeneration::prepareInitialCondition(const ros::Time path_time) {
+     487             : 
+     488          11 :   if (dont_prepend_initial_condition_) {
+     489           0 :     return {{}, false, 0};
+     490             :   }
+     491             : 
+     492          11 :   if (!sh_tracker_cmd_.hasMsg()) {
+     493           8 :     return {{}, false, 0};
+     494             :   }
+     495             : 
+     496          14 :   auto tracker_cmd = sh_tracker_cmd_.getMsg();
+     497             : 
+     498             :   // | ------------- prepare the initial conditions ------------- |
+     499             : 
+     500          14 :   mrs_msgs::TrackerCommand initial_condition;
+     501             : 
+     502           7 :   bool path_from_future = false;
+     503             : 
+     504             :   // positive = in the future
+     505           7 :   double path_time_offset = 0;
+     506             : 
+     507           7 :   if (path_time != ros::Time(0)) {
+     508           0 :     path_time_offset = (path_time - ros::Time::now()).toSec();
+     509             :   }
+     510             : 
+     511           7 :   int path_sample_offset = 0;
+     512             : 
+     513             :   // if the desired path starts in the future, more than one MPC step ahead
+     514           7 :   if (path_time_offset > 0.2) {
+     515             : 
+     516           0 :     ROS_INFO("[TrajectoryGeneration]: desired path is from the future by %.2f s", path_time_offset);
+     517             : 
+     518             :     // calculate the offset in samples in the predicted trajectory
+     519             :     // 0.01 is subtracted for the first sample, which is smaller
+     520             :     // +1 is added due to the first sample, which was subtarcted
+     521           0 :     path_sample_offset = int(ceil((path_time_offset * FUTURIZATION_FIRST_WAYPOINT_FACTOR - 0.01) / 0.2)) + 1;
+     522             : 
+     523           0 :     if (path_sample_offset > (int(tracker_cmd->full_state_prediction.position.size()) - 1)) {
+     524             : 
+     525           0 :       ROS_ERROR("[TrajectoryGeneration]: can not extrapolate into the waypoints, using tracker_cmd instead");
+     526           0 :       initial_condition = *tracker_cmd;
+     527             : 
+     528             :     } else {
+     529             : 
+     530             :       // copy the sample from the current prediction into TrackerCommand, so that we can easily transform it
+     531           0 :       mrs_msgs::TrackerCommand full_state;
+     532             : 
+     533           0 :       full_state.header = tracker_cmd->full_state_prediction.header;
+     534             : 
+     535           0 :       full_state.position     = tracker_cmd->full_state_prediction.position.at(path_sample_offset);
+     536           0 :       full_state.velocity     = tracker_cmd->full_state_prediction.velocity.at(path_sample_offset);
+     537           0 :       full_state.acceleration = tracker_cmd->full_state_prediction.acceleration.at(path_sample_offset);
+     538           0 :       full_state.jerk         = tracker_cmd->full_state_prediction.jerk.at(path_sample_offset);
+     539             : 
+     540           0 :       full_state.heading              = tracker_cmd->full_state_prediction.heading.at(path_sample_offset);
+     541           0 :       full_state.heading_rate         = tracker_cmd->full_state_prediction.heading_rate.at(path_sample_offset);
+     542           0 :       full_state.heading_acceleration = tracker_cmd->full_state_prediction.heading_acceleration.at(path_sample_offset);
+     543           0 :       full_state.heading_jerk         = tracker_cmd->full_state_prediction.heading_jerk.at(path_sample_offset);
+     544             : 
+     545           0 :       ROS_INFO("[TrajectoryGeneration]: getting initial condition from the %d-th sample of the MPC prediction", path_sample_offset);
+     546             : 
+     547           0 :       initial_condition.header = full_state.header;
+     548             : 
+     549           0 :       initial_condition.position     = full_state.position;
+     550           0 :       initial_condition.velocity     = full_state.velocity;
+     551           0 :       initial_condition.acceleration = full_state.acceleration;
+     552           0 :       initial_condition.jerk         = full_state.jerk;
+     553             : 
+     554           0 :       initial_condition.heading              = full_state.heading;
+     555           0 :       initial_condition.heading_rate         = full_state.heading_rate;
+     556           0 :       initial_condition.heading_acceleration = full_state.heading_acceleration;
+     557           0 :       initial_condition.heading_jerk         = full_state.heading_jerk;
+     558             : 
+     559           0 :       path_from_future = true;
+     560             :     }
+     561             : 
+     562             :   } else {
+     563             : 
+     564           7 :     ROS_INFO("[TrajectoryGeneration]: desired path is NOT from the future, using tracker_cmd as the initial condition");
+     565             : 
+     566           7 :     initial_condition = *tracker_cmd;
+     567             :   }
+     568             : 
+     569           7 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     570             : 
+     571           7 :   if (path_time == ros::Time(0)) {
+     572           7 :     if (!control_manager_diag->tracker_status.have_goal) {
+     573           3 :       initial_condition.header.stamp = ros::Time(0);
+     574             :     }
+     575             :   }
+     576             : 
+     577           7 :   return {{initial_condition}, path_from_future, path_sample_offset};
+     578             : }
+     579             : 
+     580             : //}
+     581             : 
+     582             : /* optimize() //{ */
+     583             : 
+     584          11 : std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> MrsTrajectoryGeneration::optimize(const std::vector<Waypoint_t>& waypoints_in,
+     585             :                                                                                                const std_msgs::Header&        waypoints_header,
+     586             :                                                                                                const bool fallback_sampling, const bool relax_heading) {
+     587             : 
+     588          33 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::optimize", scope_timer_logger_, scope_timer_enabled_);
+     589             : 
+     590          11 :   ros::Time optimize_time_start = ros::Time::now();
+     591             : 
+     592             :   // | ---------------- reset the visual markers ---------------- |
+     593             : 
+     594          11 :   bw_original_.clearBuffers();
+     595          11 :   bw_original_.clearVisuals();
+     596          11 :   bw_final_.clearBuffers();
+     597          11 :   bw_final_.clearVisuals();
+     598             : 
+     599          11 :   bw_original_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     600          11 :   bw_final_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     601             : 
+     602          11 :   bw_original_.setPointsScale(0.4);
+     603          11 :   bw_final_.setPointsScale(0.35);
+     604             : 
+     605             :   // empty path is invalid
+     606          11 :   if (waypoints_in.size() == 0) {
+     607           0 :     std::stringstream ss;
+     608           0 :     ss << "the path is empty (before postprocessing)";
+     609           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     610           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     611             :   }
+     612             : 
+     613          22 :   std::vector<Waypoint_t> waypoints_in_with_init = waypoints_in;
+     614             : 
+     615          11 :   double path_time_offset = (waypoints_header.stamp - ros::Time::now()).toSec();
+     616             : 
+     617          11 :   if (path_time_offset > 0.2 && waypoints_in_with_init.size() >= 2) {
+     618           0 :     waypoints_in_with_init.erase(waypoints_in_with_init.begin());
+     619             :   }
+     620             : 
+     621          22 :   auto [initial_condition, path_from_future, path_sample_offset] = prepareInitialCondition(waypoints_header.stamp);
+     622             : 
+     623             :   // prepend the initial condition
+     624          11 :   if (initial_condition) {
+     625             : 
+     626           7 :     Waypoint_t initial_waypoint;
+     627             :     initial_waypoint.coords =
+     628           7 :         Eigen::Vector4d(initial_condition->position.x, initial_condition->position.y, initial_condition->position.z, initial_condition->heading);
+     629           7 :     initial_waypoint.stop_at = false;
+     630           7 :     waypoints_in_with_init.insert(waypoints_in_with_init.begin(), initial_waypoint);
+     631             : 
+     632             :   } else {
+     633           4 :     if (!dont_prepend_initial_condition_) {
+     634           4 :       fly_now_ = false;
+     635             :     }
+     636             :   }
+     637             : 
+     638          22 :   std::vector<Waypoint_t> waypoints = preprocessPath(waypoints_in_with_init);
+     639             : 
+     640          11 :   if (waypoints.size() <= 1) {
+     641           0 :     std::stringstream ss;
+     642           0 :     ss << "the path is empty (after postprocessing)";
+     643           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     644           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     645             :   }
+     646             : 
+     647          11 :   bool              safe = false;
+     648             :   int               traj_idx;
+     649          22 :   std::vector<bool> segment_safeness;
+     650          11 :   double            max_deviation = 0;
+     651             : 
+     652          22 :   eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory;
+     653             : 
+     654          11 :   double sampling_dt = 0;
+     655             : 
+     656          11 :   if (path_from_future) {
+     657           0 :     ROS_INFO("[TrajectoryGeneration]: changing dt = 0.2, cause the path is from the future");
+     658           0 :     sampling_dt = 0.2;
+     659             :   } else {
+     660          11 :     sampling_dt = _sampling_dt_;
+     661             :   }
+     662             : 
+     663          11 :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> result;
+     664             : 
+     665          22 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+     666             : 
+     667          11 :   if (params.enforce_fallback_solver) {
+     668           0 :     ROS_WARN("[TrajectoryGeneration]: fallback sampling enforced");
+     669           0 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     670          11 :   } else if (fallback_sampling) {
+     671           0 :     ROS_WARN("[TrajectoryGeneration]: executing fallback sampling");
+     672           0 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     673          11 :   } else if (running_async_planning_) {
+     674           5 :     ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     675           5 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     676           6 :   } else if (overtime()) {
+     677           1 :     ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, we are running over time");
+     678           1 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     679             :   } else {
+     680             : 
+     681           5 :     result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     682             :   }
+     683             : 
+     684          11 :   if (result) {
+     685          11 :     trajectory = result.value();
+     686             :   } else {
+     687           0 :     std::stringstream ss;
+     688           0 :     ss << "failed to find trajectory";
+     689           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     690           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     691             :   }
+     692             : 
+     693          17 :   for (int k = 0; k < _trajectory_max_segment_deviation_max_iterations_; k++) {
+     694             : 
+     695          17 :     ROS_DEBUG("[TrajectoryGeneration]: revalidation cycle #%d", k);
+     696             : 
+     697          17 :     std::tie(safe, traj_idx, segment_safeness, max_deviation) = validateTrajectorySpatial(trajectory, waypoints);
+     698             : 
+     699          17 :     if (_trajectory_max_segment_deviation_enabled_ && !safe) {
+     700             : 
+     701          11 :       ROS_DEBUG("[TrajectoryGeneration]: trajectory is not safe, max deviation %.3f m", max_deviation);
+     702             : 
+     703          11 :       std::vector<Waypoint_t>::iterator waypoint = waypoints.begin();
+     704          11 :       std::vector<bool>::iterator       safeness = segment_safeness.begin();
+     705             : 
+     706         109 :       for (; waypoint < waypoints.end() - 1; waypoint++) {
+     707             : 
+     708          98 :         if (!(*safeness)) {
+     709             : 
+     710          49 :           if (waypoint > waypoints.begin() || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+     711          49 :             Waypoint_t midpoint2 = interpolatePoint(*waypoint, *(waypoint + 1), 0.5);
+     712          49 :             waypoint             = waypoints.insert(waypoint + 1, midpoint2);
+     713             :           }
+     714             :         }
+     715             : 
+     716          98 :         safeness++;
+     717             :       }
+     718             : 
+     719          11 :       if (params.enforce_fallback_solver) {
+     720           0 :         ROS_WARN("[TrajectoryGeneration]: fallback sampling enforced");
+     721           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     722          11 :       } else if (fallback_sampling) {
+     723           0 :         ROS_WARN("[TrajectoryGeneration]: executing fallback sampling");
+     724           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     725          11 :       } else if (running_async_planning_) {
+     726           0 :         ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     727           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     728          11 :       } else if (overtime()) {
+     729           0 :         ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, we are running over time");
+     730           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     731             :       } else {
+     732          11 :         result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     733             :       }
+     734             : 
+     735          11 :       if (result) {
+     736           6 :         trajectory = result.value();
+     737             :       } else {
+     738           5 :         std::stringstream ss;
+     739           5 :         ss << "failed to find trajectory";
+     740           5 :         ROS_WARN_STREAM("[TrajectoryGeneration]: " << ss.str());
+     741           5 :         return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     742           6 :       }
+     743             : 
+     744             :     } else {
+     745           6 :       ROS_DEBUG("[TrajectoryGeneration]: trajectory is safe (%.2f)", max_deviation);
+     746           6 :       safe = true;
+     747           6 :       break;
+     748             :     }
+     749             :   }
+     750             : 
+     751           6 :   ROS_INFO("[TrajectoryGeneration]: final max trajectory-path deviation: %.2f m, total trajectory time: %.2fs ", max_deviation,
+     752             :            trajectory.size() * sampling_dt);
+     753             : 
+     754             :   // prepare rviz markers
+     755          34 :   for (int i = 0; i < int(waypoints.size()); i++) {
+     756          28 :     bw_final_.addPoint(vec3_t(waypoints.at(i).coords(0), waypoints.at(i).coords(1), waypoints.at(i).coords(2)), 0.0, 1.0, 0.0, 1.0);
+     757             :   }
+     758             : 
+     759          12 :   mrs_msgs::TrajectoryReference mrs_trajectory;
+     760             : 
+     761             :   // convert the optimized trajectory to mrs_msgs::TrajectoryReference
+     762           6 :   mrs_trajectory = getTrajectoryReference(trajectory, initial_condition, sampling_dt);
+     763             : 
+     764             :   // insert part of the MPC prediction in the front of the generated trajectory to compensate for the future
+     765           6 :   if (path_from_future) {
+     766             : 
+     767           0 :     auto current_prediction = sh_tracker_cmd_.getMsg()->full_state_prediction;
+     768             : 
+     769             :     // calculate the starting idx that we will use from the current_prediction
+     770           0 :     double path_time_offset_2   = (ros::Time::now() - current_prediction.header.stamp).toSec();  // = how long did it take to optimize
+     771           0 :     int    path_sample_offset_2 = int(floor((path_time_offset_2 - 0.01) / 0.2)) + 1;
+     772             : 
+     773             :     // if there is anything to insert
+     774           0 :     if (path_sample_offset > path_sample_offset_2) {
+     775             : 
+     776           0 :       ROS_INFO("[TrajectoryGeneration]: inserting pre-trajectory from the prediction, idxs %d to %d", path_sample_offset_2, path_sample_offset);
+     777             : 
+     778           0 :       for (int i = path_sample_offset - 1; i >= 0; i--) {
+     779             : 
+     780           0 :         ROS_DEBUG("[TrajectoryGeneration]: inserting idx %d", i);
+     781             : 
+     782           0 :         mrs_msgs::ReferenceStamped reference;
+     783             : 
+     784           0 :         reference.header = current_prediction.header;
+     785             : 
+     786           0 :         reference.reference.heading  = current_prediction.heading.at(i);
+     787           0 :         reference.reference.position = current_prediction.position.at(i);
+     788             : 
+     789           0 :         auto res = transformer_->transformSingle(reference, waypoints_header.frame_id);
+     790             : 
+     791           0 :         if (res) {
+     792           0 :           reference = res.value();
+     793             :         } else {
+     794           0 :           std::stringstream ss;
+     795           0 :           ss << "could not transform reference to the path frame";
+     796           0 :           ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     797           0 :           return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     798             :         }
+     799             : 
+     800           0 :         mrs_trajectory.points.insert(mrs_trajectory.points.begin(), reference.reference);
+     801             :       }
+     802             :     }
+     803             :   }
+     804             : 
+     805           6 :   bw_original_.publish();
+     806           6 :   bw_final_.publish();
+     807             : 
+     808           6 :   std::stringstream ss;
+     809           6 :   ss << "trajectory generated";
+     810             : 
+     811           6 :   ROS_DEBUG("[TrajectoryGeneration]: trajectory generated, took %.3f s", (ros::Time::now() - optimize_time_start).toSec());
+     812             : 
+     813           6 :   return std::tuple(true, ss.str(), mrs_trajectory);
+     814             : }
+     815             : 
+     816             : //}
+     817             : 
+     818             : /* findTrajectory() //{ */
+     819             : 
+     820          16 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectory(const std::vector<Waypoint_t>&                 waypoints,
+     821             :                                                                                                   const std::optional<mrs_msgs::TrackerCommand>& initial_state,
+     822             :                                                                                                   const double& sampling_dt, const bool& relax_heading) {
+     823             : 
+     824          48 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectory", scope_timer_logger_, scope_timer_enabled_);
+     825             : 
+     826          32 :   mrs_lib::AtomicScopeFlag unset_running(running_async_planning_);
+     827             : 
+     828          16 :   ROS_DEBUG("[TrajectoryGeneration]: findTrajectory() started");
+     829             : 
+     830          16 :   ros::Time find_trajectory_time_start = ros::Time::now();
+     831             : 
+     832          32 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+     833          32 :   auto constraints = sh_constraints_.getMsg();
+     834             : 
+     835          32 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     836             : 
+     837          16 :   if (initial_state && (initial_state->header.stamp - ros::Time::now()).toSec() < 0.2 && control_manager_diag->tracker_status.have_goal) {
+     838           3 :     max_deviation_first_segment_ = false;
+     839             :   } else {
+     840          13 :     max_deviation_first_segment_ = true;
+     841             :   }
+     842             : 
+     843             :   // optimizer
+     844             : 
+     845          16 :   eth_trajectory_generation::NonlinearOptimizationParameters parameters;
+     846             : 
+     847          16 :   parameters.f_rel                  = 0.05;
+     848          16 :   parameters.x_rel                  = 0.1;
+     849          16 :   parameters.time_penalty           = params.time_penalty;
+     850          16 :   parameters.use_soft_constraints   = params.soft_constraints_enabled;
+     851          16 :   parameters.soft_constraint_weight = params.soft_constraints_weight;
+     852          16 :   parameters.time_alloc_method      = static_cast<eth_trajectory_generation::NonlinearOptimizationParameters::TimeAllocMethod>(params.time_allocation);
+     853          16 :   if (params.time_allocation == 2) {
+     854          16 :     parameters.algorithm = nlopt::LD_LBFGS;
+     855             :   }
+     856          16 :   parameters.initial_stepsize_rel            = 0.1;
+     857          16 :   parameters.inequality_constraint_tolerance = params.inequality_constraint_tolerance;
+     858          16 :   parameters.equality_constraint_tolerance   = params.equality_constraint_tolerance;
+     859          16 :   parameters.max_iterations                  = params.max_iterations;
+     860             : 
+     861             :   // let's tell the solver tha it is more time then it thinks, to not stop prematurely
+     862          16 :   parameters.max_time = 2 * (NLOPT_EXEC_TIME_FACTOR * timeLeft());
+     863             : 
+     864          32 :   eth_trajectory_generation::Vertex::Vector vertices;
+     865          16 :   const int                                 dimension = 4;
+     866             : 
+     867          16 :   int derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     868             : 
+     869          16 :   switch (params.derivative_to_optimize) {
+     870          16 :     case 0: {
+     871          16 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     872          16 :       break;
+     873             :     }
+     874           0 :     case 1: {
+     875           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::JERK;
+     876           0 :       break;
+     877             :     }
+     878           0 :     case 2: {
+     879           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::SNAP;
+     880           0 :       break;
+     881             :     }
+     882             :   }
+     883             : 
+     884             :   // | --------------- add constraints to vertices -------------- |
+     885             : 
+     886             :   double last_heading;
+     887             : 
+     888          16 :   if (initial_state) {
+     889          12 :     last_heading = initial_state->heading;
+     890             :   } else {
+     891           4 :     last_heading = waypoints.at(0).coords(3);
+     892             :   }
+     893             : 
+     894         197 :   for (size_t i = 0; i < waypoints.size(); i++) {
+     895         181 :     double x       = waypoints.at(i).coords(0);
+     896         181 :     double y       = waypoints.at(i).coords(1);
+     897         181 :     double z       = waypoints.at(i).coords(2);
+     898         181 :     double heading = sradians::unwrap(waypoints.at(i).coords(3), last_heading);
+     899         181 :     last_heading   = heading;
+     900             : 
+     901         362 :     eth_trajectory_generation::Vertex vertex(dimension);
+     902             : 
+     903         181 :     if (i == 0) {
+     904             : 
+     905          16 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     906             : 
+     907          16 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     908             : 
+     909          16 :       if (initial_state) {
+     910             : 
+     911          12 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY,
+     912          12 :                              Eigen::Vector4d(initial_state->velocity.x, initial_state->velocity.y, initial_state->velocity.z, initial_state->heading_rate));
+     913             : 
+     914          12 :         vertex.addConstraint(
+     915             :             eth_trajectory_generation::derivative_order::ACCELERATION,
+     916          12 :             Eigen::Vector4d(initial_state->acceleration.x, initial_state->acceleration.y, initial_state->acceleration.z, initial_state->heading_acceleration));
+     917             : 
+     918          12 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK,
+     919          24 :                              Eigen::Vector4d(initial_state->jerk.x, initial_state->jerk.y, initial_state->jerk.z, initial_state->heading_jerk));
+     920             :       }
+     921             : 
+     922         165 :     } else if (i == (waypoints.size() - 1)) {  // the last point
+     923             : 
+     924          16 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     925             : 
+     926          16 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     927             : 
+     928             :     } else {  // mid points
+     929             : 
+     930         149 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     931             : 
+     932         149 :       if (waypoints.at(i).stop_at) {
+     933           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, Eigen::Vector4d(0, 0, 0, 0));
+     934           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::ACCELERATION, Eigen::Vector4d(0, 0, 0, 0));
+     935           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, Eigen::Vector4d(0, 0, 0, 0));
+     936             :       }
+     937             :     }
+     938             : 
+     939         181 :     vertices.push_back(vertex);
+     940             :   }
+     941             : 
+     942             :   // | ---------------- compute the segment times --------------- |
+     943             : 
+     944             :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+     945             :   double v_max_vertical, a_max_vertical, j_max_vertical;
+     946             : 
+     947             :   // use the small of the ascending/descending values
+     948          16 :   double vertical_speed_lim        = std::min(constraints->vertical_ascending_speed, constraints->vertical_descending_speed);
+     949          16 :   double vertical_acceleration_lim = std::min(constraints->vertical_ascending_acceleration, constraints->vertical_descending_acceleration);
+     950             : 
+     951          16 :   v_max_horizontal = constraints->horizontal_speed;
+     952          16 :   a_max_horizontal = constraints->horizontal_acceleration;
+     953             : 
+     954          16 :   v_max_vertical = vertical_speed_lim;
+     955          16 :   a_max_vertical = vertical_acceleration_lim;
+     956             : 
+     957          16 :   j_max_horizontal = constraints->horizontal_jerk;
+     958          16 :   j_max_vertical   = std::min(constraints->vertical_ascending_jerk, constraints->vertical_descending_jerk);
+     959             : 
+     960          16 :   if (override_constraints_) {
+     961             : 
+     962           0 :     bool can_change = true;
+     963             : 
+     964             : 
+     965           0 :     if (initial_state) {
+     966           0 :       can_change = (hypot(initial_state->velocity.x, initial_state->velocity.y) < override_max_velocity_horizontal_) &&
+     967           0 :                    (hypot(initial_state->acceleration.x, initial_state->acceleration.y) < override_max_acceleration_horizontal_) &&
+     968           0 :                    (hypot(initial_state->jerk.x, initial_state->jerk.y) < override_max_jerk_horizontal_) &&
+     969           0 :                    (fabs(initial_state->velocity.z) < override_max_velocity_vertical_) &&
+     970           0 :                    (fabs(initial_state->acceleration.z) < override_max_acceleration_vertical_) && (fabs(initial_state->jerk.z) < override_max_jerk_vertical_);
+     971             :     }
+     972             : 
+     973           0 :     if (can_change) {
+     974             : 
+     975           0 :       v_max_horizontal = override_max_velocity_horizontal_;
+     976           0 :       a_max_horizontal = override_max_acceleration_horizontal_;
+     977           0 :       j_max_horizontal = override_max_jerk_horizontal_;
+     978             : 
+     979           0 :       v_max_vertical = override_max_velocity_vertical_;
+     980           0 :       a_max_vertical = override_max_acceleration_vertical_;
+     981           0 :       j_max_vertical = override_max_jerk_vertical_;
+     982             : 
+     983           0 :       ROS_DEBUG("[TrajectoryGeneration]: overriding constraints by a user");
+     984             : 
+     985             :     } else {
+     986             : 
+     987           0 :       ROS_WARN("[TrajectoryGeneration]: overrifing constraints refused due to possible infeasibility");
+     988             :     }
+     989             :   }
+     990             : 
+     991             :   double v_max_heading, a_max_heading, j_max_heading;
+     992             : 
+     993          16 :   if (relax_heading) {
+     994           0 :     v_max_heading = std::numeric_limits<float>::max();
+     995           0 :     a_max_heading = std::numeric_limits<float>::max();
+     996           0 :     j_max_heading = std::numeric_limits<float>::max();
+     997             :   } else {
+     998          16 :     v_max_heading = constraints->heading_speed;
+     999          16 :     a_max_heading = constraints->heading_acceleration;
+    1000          16 :     j_max_heading = constraints->heading_jerk;
+    1001             :   }
+    1002             : 
+    1003          16 :   ROS_DEBUG("[TrajectoryGeneration]: using constraints:");
+    1004          16 :   ROS_DEBUG("[TrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+    1005          16 :   ROS_DEBUG("[TrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+    1006          16 :   ROS_DEBUG("[TrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+    1007             : 
+    1008          32 :   std::vector<double> segment_times, segment_times_baca;
+    1009          32 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1010          16 :                                        v_max_heading, a_max_heading);
+    1011          32 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1012          16 :                                                 v_max_heading, a_max_heading);
+    1013             : 
+    1014          16 :   double initial_total_time      = 0;
+    1015          16 :   double initial_total_time_baca = 0;
+    1016         181 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+    1017         165 :     initial_total_time += segment_times.at(i);
+    1018         165 :     initial_total_time_baca += segment_times_baca.at(i);
+    1019             :   }
+    1020             : 
+    1021          16 :   ROS_DEBUG("[TrajectoryGeneration]: initial total time (Euclidean): %.2f", initial_total_time);
+    1022          16 :   ROS_DEBUG("[TrajectoryGeneration]: initial total time (Baca): %.2f", initial_total_time_baca);
+    1023             : 
+    1024             :   // | --------- create an optimizer object and solve it -------- |
+    1025             : 
+    1026          16 :   const int                                                     N = 10;
+    1027          32 :   eth_trajectory_generation::PolynomialOptimizationNonLinear<N> opt(dimension, parameters);
+    1028          16 :   opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
+    1029             : 
+    1030          16 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+    1031          16 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+    1032          16 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+    1033             : 
+    1034          16 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+    1035          16 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+    1036          16 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+    1037             : 
+    1038          16 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::VELOCITY, v_max_vertical);
+    1039          16 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_vertical);
+    1040          16 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::JERK, j_max_vertical);
+    1041             : 
+    1042          16 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::VELOCITY, v_max_heading);
+    1043          16 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_heading);
+    1044          16 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::JERK, j_max_heading);
+    1045             : 
+    1046          16 :   opt.optimize();
+    1047             : 
+    1048          16 :   if (overtime()) {
+    1049           5 :     return {};
+    1050             :   }
+    1051             : 
+    1052          22 :   std::string result_str;
+    1053             : 
+    1054          11 :   switch (opt.getOptimizationInfo().stopping_reason) {
+    1055           3 :     case nlopt::FAILURE: {
+    1056           3 :       result_str = "generic failure";
+    1057           3 :       break;
+    1058             :     }
+    1059           0 :     case nlopt::INVALID_ARGS: {
+    1060           0 :       result_str = "invalid args";
+    1061           0 :       break;
+    1062             :     }
+    1063           0 :     case nlopt::OUT_OF_MEMORY: {
+    1064           0 :       result_str = "out of memory";
+    1065           0 :       break;
+    1066             :     }
+    1067           0 :     case nlopt::ROUNDOFF_LIMITED: {
+    1068           0 :       result_str = "roundoff limited";
+    1069           0 :       break;
+    1070             :     }
+    1071           0 :     case nlopt::FORCED_STOP: {
+    1072           0 :       result_str = "forced stop";
+    1073           0 :       break;
+    1074             :     }
+    1075           0 :     case nlopt::STOPVAL_REACHED: {
+    1076           0 :       result_str = "stopval reached";
+    1077           0 :       break;
+    1078             :     }
+    1079           2 :     case nlopt::FTOL_REACHED: {
+    1080           2 :       result_str = "ftol reached";
+    1081           2 :       break;
+    1082             :     }
+    1083           3 :     case nlopt::XTOL_REACHED: {
+    1084           3 :       result_str = "xtol reached";
+    1085           3 :       break;
+    1086             :     }
+    1087           3 :     case nlopt::MAXEVAL_REACHED: {
+    1088           3 :       result_str = "maxeval reached";
+    1089           3 :       break;
+    1090             :     }
+    1091           0 :     case nlopt::MAXTIME_REACHED: {
+    1092           0 :       result_str = "maxtime reached";
+    1093           0 :       break;
+    1094             :     }
+    1095           0 :     default: {
+    1096           0 :       result_str = "UNKNOWN FAILURE CODE";
+    1097           0 :       break;
+    1098             :     }
+    1099             :   }
+    1100             : 
+    1101          11 :   if (opt.getOptimizationInfo().stopping_reason >= 1 && opt.getOptimizationInfo().stopping_reason != 6) {
+    1102           8 :     ROS_DEBUG("[TrajectoryGeneration]: optimization finished successfully with code %d, '%s'", opt.getOptimizationInfo().stopping_reason, result_str.c_str());
+    1103             : 
+    1104           3 :   } else if (opt.getOptimizationInfo().stopping_reason == -1) {
+    1105           3 :     ROS_DEBUG("[TrajectoryGeneration]: optimization finished with a generic error code %d, '%s'", opt.getOptimizationInfo().stopping_reason,
+    1106             :               result_str.c_str());
+    1107             : 
+    1108             :   } else {
+    1109           0 :     ROS_WARN("[TrajectoryGeneration]: optimization failed with code %d, '%s', took %.3f s", opt.getOptimizationInfo().stopping_reason, result_str.c_str(),
+    1110             :              (ros::Time::now() - find_trajectory_time_start).toSec());
+    1111           0 :     return {};
+    1112             :   }
+    1113             : 
+    1114             :   // | ------------- obtain the polynomial segments ------------- |
+    1115             : 
+    1116          22 :   eth_trajectory_generation::Segment::Vector segments;
+    1117          11 :   opt.getPolynomialOptimizationRef().getSegments(&segments);
+    1118             : 
+    1119          11 :   if (overtime()) {
+    1120           0 :     return {};
+    1121             :   }
+    1122             : 
+    1123             :   // | --------------- create the trajectory class -------------- |
+    1124             : 
+    1125          22 :   eth_trajectory_generation::Trajectory trajectory;
+    1126          11 :   opt.getTrajectory(&trajectory);
+    1127             : 
+    1128          22 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1129             : 
+    1130          11 :   ROS_DEBUG("[TrajectoryGeneration]: starting eth sampling with dt = %.2f s ", sampling_dt);
+    1131             : 
+    1132          11 :   bool success = eth_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_dt, &states);
+    1133             : 
+    1134          11 :   if (overtime()) {
+    1135           0 :     return {};
+    1136             :   }
+    1137             : 
+    1138             :   // validate the temporal sampling of the trajectory
+    1139             : 
+    1140             :   // only check this if the trajectory is > 1.0 sec, this check does not make much sense for the short ones
+    1141          11 :   if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) > (_max_trajectory_len_factor_ * initial_total_time_baca)) {
+    1142           0 :     ROS_ERROR("[TrajectoryGeneration]: the final trajectory sampling is too long = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1143             :               (states.size() * sampling_dt), initial_total_time_baca, _max_trajectory_len_factor_);
+    1144             : 
+    1145           0 :     std::stringstream ss;
+    1146           0 :     ss << "trajectory sampling failed";
+    1147           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+    1148           0 :     return {};
+    1149             : 
+    1150          11 :   } else if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) < (_min_trajectory_len_factor_ * initial_total_time_baca)) {
+    1151           0 :     ROS_ERROR("[TrajectoryGeneration]: the final trajectory sampling is too short = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1152             :               (states.size() * sampling_dt), initial_total_time_baca, _min_trajectory_len_factor_);
+    1153             : 
+    1154           0 :     std::stringstream ss;
+    1155           0 :     ss << "trajectory sampling failed";
+    1156           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+    1157           0 :     return {};
+    1158             : 
+    1159             :   } else {
+    1160          11 :     ROS_DEBUG("[TrajectoryGeneration]: estimated/final trajectory length ratio (final/estimated) %.2f",
+    1161             :               (states.size() * sampling_dt) / initial_total_time_baca);
+    1162             :   }
+    1163             : 
+    1164          11 :   if (success) {
+    1165          11 :     ROS_DEBUG("[TrajectoryGeneration]: eth sampling finished, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1166          11 :     return std::optional(states);
+    1167             : 
+    1168             :   } else {
+    1169           0 :     ROS_ERROR("[TrajectoryGeneration]: eth could not sample the trajectory, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1170           0 :     return {};
+    1171             :   }
+    1172             : }
+    1173             : 
+    1174             : //}
+    1175             : 
+    1176             : /* findTrajectoryFallback() //{ */
+    1177             : 
+    1178           6 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryFallback(const std::vector<Waypoint_t>& waypoints,
+    1179             :                                                                                                           const double&                  sampling_dt,
+    1180             :                                                                                                           const bool&                    relax_heading) {
+    1181             : 
+    1182          18 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectoryFallback", scope_timer_logger_, scope_timer_enabled_);
+    1183             : 
+    1184           6 :   ros::Time time_start = ros::Time::now();
+    1185             : 
+    1186           6 :   ROS_WARN("[TrajectoryGeneration]: fallback sampling started");
+    1187             : 
+    1188          12 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+    1189          12 :   auto constraints = sh_constraints_.getMsg();
+    1190             : 
+    1191          12 :   eth_trajectory_generation::Vertex::Vector vertices;
+    1192           6 :   const int                                 dimension = 4;
+    1193             : 
+    1194             :   // | --------------- add constraints to vertices -------------- |
+    1195             : 
+    1196           6 :   double last_heading = waypoints.at(0).coords(3);
+    1197             : 
+    1198          34 :   for (size_t i = 0; i < waypoints.size(); i++) {
+    1199             : 
+    1200          28 :     double x       = waypoints.at(i).coords(0);
+    1201          28 :     double y       = waypoints.at(i).coords(1);
+    1202          28 :     double z       = waypoints.at(i).coords(2);
+    1203          28 :     double heading = sradians::unwrap(waypoints.at(i).coords(3), last_heading);
+    1204          28 :     last_heading   = heading;
+    1205             : 
+    1206          56 :     eth_trajectory_generation::Vertex vertex(dimension);
+    1207             : 
+    1208          28 :     vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+    1209             : 
+    1210          28 :     vertices.push_back(vertex);
+    1211             :   }
+    1212             : 
+    1213             :   // | ---------------- compute the segment times --------------- |
+    1214             : 
+    1215             :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+    1216             :   double v_max_vertical, a_max_vertical, j_max_vertical;
+    1217             : 
+    1218             :   // use the small of the ascending/descending values
+    1219           6 :   double vertical_speed_lim        = std::min(constraints->vertical_ascending_speed, constraints->vertical_descending_speed);
+    1220           6 :   double vertical_acceleration_lim = std::min(constraints->vertical_ascending_acceleration, constraints->vertical_descending_acceleration);
+    1221             : 
+    1222           6 :   if (override_constraints_) {
+    1223             : 
+    1224           0 :     v_max_horizontal = override_max_velocity_horizontal_;
+    1225           0 :     a_max_horizontal = override_max_acceleration_horizontal_;
+    1226           0 :     j_max_horizontal = override_max_jerk_horizontal_;
+    1227             : 
+    1228           0 :     v_max_vertical = override_max_velocity_vertical_;
+    1229           0 :     a_max_vertical = override_max_acceleration_vertical_;
+    1230           0 :     j_max_vertical = override_max_jerk_vertical_;
+    1231             : 
+    1232           0 :     ROS_DEBUG("[TrajectoryGeneration]: overriding constraints by a user");
+    1233             :   } else {
+    1234             : 
+    1235           6 :     v_max_horizontal = constraints->horizontal_speed;
+    1236           6 :     a_max_horizontal = constraints->horizontal_acceleration;
+    1237             : 
+    1238           6 :     v_max_vertical = vertical_speed_lim;
+    1239           6 :     a_max_vertical = vertical_acceleration_lim;
+    1240             : 
+    1241           6 :     j_max_horizontal = constraints->horizontal_jerk;
+    1242           6 :     j_max_vertical   = std::min(constraints->vertical_ascending_jerk, constraints->vertical_descending_jerk);
+    1243             :   }
+    1244             : 
+    1245             : 
+    1246             :   double v_max_heading, a_max_heading, j_max_heading;
+    1247             : 
+    1248           6 :   if (relax_heading) {
+    1249           0 :     v_max_heading = std::numeric_limits<float>::max();
+    1250           0 :     a_max_heading = std::numeric_limits<float>::max();
+    1251           0 :     j_max_heading = std::numeric_limits<float>::max();
+    1252             :   } else {
+    1253           6 :     v_max_heading = constraints->heading_speed;
+    1254           6 :     a_max_heading = constraints->heading_acceleration;
+    1255           6 :     j_max_heading = constraints->heading_jerk;
+    1256             :   }
+    1257             : 
+    1258           6 :   v_max_horizontal *= _fallback_sampling_speed_factor_;
+    1259           6 :   v_max_vertical *= _fallback_sampling_speed_factor_;
+    1260             : 
+    1261           6 :   a_max_horizontal *= _fallback_sampling_accel_factor_;
+    1262           6 :   a_max_vertical *= _fallback_sampling_accel_factor_;
+    1263             : 
+    1264           6 :   ROS_DEBUG("[TrajectoryGeneration]: using constraints:");
+    1265           6 :   ROS_DEBUG("[TrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+    1266           6 :   ROS_DEBUG("[TrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+    1267           6 :   ROS_DEBUG("[TrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+    1268             : 
+    1269          12 :   std::vector<double> segment_times, segment_times_baca;
+    1270          12 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1271           6 :                                        v_max_heading, a_max_heading);
+    1272          12 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1273           6 :                                                 v_max_heading, a_max_heading);
+    1274             : 
+    1275           6 :   double initial_total_time      = 0;
+    1276           6 :   double initial_total_time_baca = 0;
+    1277          28 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+    1278          22 :     initial_total_time += segment_times.at(i);
+    1279          22 :     initial_total_time_baca += segment_times_baca.at(i);
+    1280             : 
+    1281          22 :     ROS_DEBUG("[TrajectoryGeneration]: segment time [%d] = %.2f", i, segment_times_baca.at(i));
+    1282             :   }
+    1283             : 
+    1284           6 :   ROS_WARN("[TrajectoryGeneration]: fallback: initial total time (Euclidean): %.2f", initial_total_time);
+    1285           6 :   ROS_WARN("[TrajectoryGeneration]: fallback: initial total time (Baca): %.2f", initial_total_time_baca);
+    1286             : 
+    1287          12 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1288             : 
+    1289             :   // interpolate each segment
+    1290          28 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1291             : 
+    1292          44 :     Eigen::VectorXd start, end;
+    1293             : 
+    1294          22 :     const double segment_time = segment_times_baca.at(i);
+    1295             : 
+    1296             :     int    n_samples;
+    1297             :     double interp_step;
+    1298             : 
+    1299          22 :     if (segment_time > 1e-1) {
+    1300             : 
+    1301          22 :       n_samples = ceil(segment_time / sampling_dt);
+    1302             : 
+    1303             :       // important
+    1304          22 :       if (n_samples > 0) {
+    1305          22 :         interp_step = 1.0 / double(n_samples);
+    1306             :       } else {
+    1307           0 :         interp_step = 0.5;
+    1308             :       }
+    1309             : 
+    1310             :     } else {
+    1311           0 :       n_samples   = 0;
+    1312           0 :       interp_step = 0;
+    1313             :     }
+    1314             : 
+    1315          22 :     ROS_DEBUG("[TrajectoryGeneration]: segment n_samples [%lu] = %d", i, n_samples);
+    1316             : 
+    1317             :     // for the last segment, hit the last waypoint completely
+    1318             :     // otherwise, it is hit as the first sample of the following segment
+    1319          22 :     if (n_samples > 0 && i == waypoints.size() - 2) {
+    1320           6 :       n_samples++;
+    1321             :     }
+    1322             : 
+    1323         871 :     for (int j = 0; j < n_samples; j++) {
+    1324             : 
+    1325         849 :       Waypoint_t point = interpolatePoint(waypoints.at(i), waypoints.at(i + 1), j * interp_step);
+    1326             : 
+    1327         849 :       eth_mav_msgs::EigenTrajectoryPoint eth_point;
+    1328         849 :       eth_point.position_W(0) = point.coords(0);
+    1329         849 :       eth_point.position_W(1) = point.coords(1);
+    1330         849 :       eth_point.position_W(2) = point.coords(2);
+    1331         849 :       eth_point.setFromYaw(point.coords(3));
+    1332             : 
+    1333         849 :       states.push_back(eth_point);
+    1334             : 
+    1335         849 :       if (j == 0 && i > 0 && waypoints.at(i).stop_at) {
+    1336             : 
+    1337           0 :         int insert_samples = int(round(_fallback_sampling_stopping_time_ / sampling_dt));
+    1338             : 
+    1339           0 :         for (int k = 0; k < insert_samples; k++) {
+    1340           0 :           states.push_back(eth_point);
+    1341             :         }
+    1342             :       }
+    1343             :     }
+    1344             :   }
+    1345             : 
+    1346           6 :   bool success = true;
+    1347             : 
+    1348           6 :   ROS_WARN("[TrajectoryGeneration]: fallback: sampling finished, took %.3f s", (ros::Time::now() - time_start).toSec());
+    1349             : 
+    1350             :   // | --------------- create the trajectory class -------------- |
+    1351             : 
+    1352           6 :   if (success) {
+    1353           6 :     return std::optional(states);
+    1354             :   } else {
+    1355           0 :     ROS_ERROR("[TrajectoryGeneration]: fallback: sampling failed");
+    1356           0 :     return {};
+    1357             :   }
+    1358             : }
+    1359             : 
+    1360             : //}
+    1361             : 
+    1362             : /* validateTrajectorySpatial() //{ */
+    1363             : 
+    1364          17 : std::tuple<bool, int, std::vector<bool>, double> MrsTrajectoryGeneration::validateTrajectorySpatial(
+    1365             :     const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector<Waypoint_t>& waypoints) {
+    1366             : 
+    1367          51 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::validateTrajectorySpatial", scope_timer_logger_, scope_timer_enabled_);
+    1368             : 
+    1369             :   // prepare the output
+    1370             : 
+    1371          17 :   std::vector<bool> segments;
+    1372         137 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1373         120 :     segments.push_back(true);
+    1374             :   }
+    1375             : 
+    1376          17 :   int waypoint_idx = 0;
+    1377             : 
+    1378          17 :   bool   is_safe       = true;
+    1379          17 :   double max_deviation = 0;
+    1380             : 
+    1381        2257 :   for (size_t i = 0; i < trajectory.size() - 1; i++) {
+    1382             : 
+    1383             :     // the trajectory sample
+    1384        2240 :     const vec3_t sample = vec3_t(trajectory.at(i).position_W(0), trajectory.at(i).position_W(1), trajectory.at(i).position_W(2));
+    1385             : 
+    1386             :     // next sample
+    1387        2240 :     const vec3_t next_sample = vec3_t(trajectory.at(i + 1).position_W(0), trajectory.at(i + 1).position_W(1), trajectory.at(i + 1).position_W(2));
+    1388             : 
+    1389             :     // segment start
+    1390        2240 :     const vec3_t segment_start = vec3_t(waypoints.at(waypoint_idx).coords(0), waypoints.at(waypoint_idx).coords(1), waypoints.at(waypoint_idx).coords(2));
+    1391             : 
+    1392             :     // segment end
+    1393             :     const vec3_t segment_end =
+    1394        2240 :         vec3_t(waypoints.at(waypoint_idx + 1).coords(0), waypoints.at(waypoint_idx + 1).coords(1), waypoints.at(waypoint_idx + 1).coords(2));
+    1395             : 
+    1396        2240 :     const double distance_from_segment = distFromSegment(sample, segment_start, segment_end);
+    1397             : 
+    1398        2240 :     const double segment_end_dist = distFromSegment(segment_end, sample, next_sample);
+    1399             : 
+    1400        2240 :     if (waypoint_idx > 0 || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+    1401             : 
+    1402        2086 :       if (distance_from_segment > max_deviation) {
+    1403         307 :         max_deviation = distance_from_segment;
+    1404             :       }
+    1405             : 
+    1406        2086 :       if (distance_from_segment > trajectory_max_segment_deviation_) {
+    1407         798 :         segments.at(waypoint_idx) = false;
+    1408         798 :         is_safe                   = false;
+    1409             :       }
+    1410             :     }
+    1411             : 
+    1412        2240 :     if (segment_end_dist < 0.05 && waypoint_idx < (int(waypoints.size()) - 2)) {
+    1413         103 :       waypoint_idx++;
+    1414             :     }
+    1415             :   }
+    1416             : 
+    1417          51 :   return std::tuple(is_safe, trajectory.size(), segments, max_deviation);
+    1418             : }
+    1419             : 
+    1420             : //}
+    1421             : 
+    1422             : // | --------------------- minor routines --------------------- |
+    1423             : 
+    1424             : /* findTrajectoryAsync() //{ */
+    1425             : 
+    1426          16 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryAsync(
+    1427             :     const std::vector<Waypoint_t>& waypoints, const std::optional<mrs_msgs::TrackerCommand>& initial_state, const double& sampling_dt,
+    1428             :     const bool& relax_heading) {
+    1429             : 
+    1430          16 :   ROS_DEBUG("[TrajectoryGeneration]: starting the async planning task");
+    1431             : 
+    1432             :   future_trajectory_result_ =
+    1433          16 :       std::async(std::launch::async, &MrsTrajectoryGeneration::findTrajectory, this, waypoints, initial_state, sampling_dt, relax_heading);
+    1434             : 
+    1435        9716 :   while (ros::ok() && future_trajectory_result_.wait_for(std::chrono::milliseconds(1)) != std::future_status::ready) {
+    1436             : 
+    1437        9705 :     if (overtime()) {
+    1438           5 :       ROS_WARN("[TrajectoryGeneration]: async task planning timeout, breaking");
+    1439           5 :       return {};
+    1440             :     }
+    1441             :   }
+    1442             : 
+    1443          11 :   ROS_DEBUG("[TrajectoryGeneration]: async planning task finished successfully");
+    1444             : 
+    1445          11 :   return future_trajectory_result_.get();
+    1446             : }
+    1447             : 
+    1448             : //}
+    1449             : 
+    1450             : /* distFromSegment() //{ */
+    1451             : 
+    1452        4480 : double MrsTrajectoryGeneration::distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2) {
+    1453             : 
+    1454        4480 :   vec3_t segment_vector = seg2 - seg1;
+    1455        4480 :   double segment_len    = segment_vector.norm();
+    1456             : 
+    1457        4480 :   vec3_t segment_vector_norm = segment_vector;
+    1458        4480 :   segment_vector_norm.normalize();
+    1459             : 
+    1460        4480 :   double point_coordinate = segment_vector_norm.dot(point - seg1);
+    1461             : 
+    1462        4480 :   if (point_coordinate < 0) {
+    1463          25 :     return (point - seg1).norm();
+    1464        4455 :   } else if (point_coordinate > segment_len) {
+    1465        2175 :     return (point - seg2).norm();
+    1466             :   } else {
+    1467             : 
+    1468        2280 :     mat3_t segment_projector = segment_vector_norm * segment_vector_norm.transpose();
+    1469        2280 :     vec3_t projection        = seg1 + segment_projector * (point - seg1);
+    1470             : 
+    1471        2280 :     return (point - projection).norm();
+    1472             :   }
+    1473             : }
+    1474             : 
+    1475             : //}
+    1476             : 
+    1477             : /* getTrajectoryReference() //{ */
+    1478             : 
+    1479           6 : mrs_msgs::TrajectoryReference MrsTrajectoryGeneration::getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+    1480             :                                                                               const std::optional<mrs_msgs::TrackerCommand>&    initial_condition,
+    1481             :                                                                               const double&                                     sampling_dt) {
+    1482             : 
+    1483           6 :   mrs_msgs::TrajectoryReference msg;
+    1484             : 
+    1485           6 :   if (initial_condition) {
+    1486           4 :     msg.header.stamp = initial_condition->header.stamp;
+    1487             :   } else {
+    1488           2 :     msg.header.stamp = ros::Time::now();
+    1489             :   }
+    1490             : 
+    1491           6 :   msg.header.frame_id = frame_id_;
+    1492           6 :   msg.fly_now         = fly_now_;
+    1493           6 :   msg.loop            = loop_;
+    1494           6 :   msg.use_heading     = use_heading_;
+    1495           6 :   msg.dt              = sampling_dt;
+    1496             : 
+    1497         855 :   for (size_t it = 0; it < trajectory.size(); it++) {
+    1498             : 
+    1499         849 :     mrs_msgs::Reference point;
+    1500         849 :     point.heading    = 0;
+    1501         849 :     point.position.x = trajectory.at(it).position_W(0);
+    1502         849 :     point.position.y = trajectory.at(it).position_W(1);
+    1503         849 :     point.position.z = trajectory.at(it).position_W(2);
+    1504         849 :     point.heading    = trajectory.at(it).getYaw();
+    1505             : 
+    1506         849 :     msg.points.push_back(point);
+    1507             :   }
+    1508             : 
+    1509           6 :   return msg;
+    1510             : }
+    1511             : 
+    1512             : //}
+    1513             : 
+    1514             : /* interpolatePoint() //{ */
+    1515             : 
+    1516         898 : Waypoint_t MrsTrajectoryGeneration::interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff) {
+    1517             : 
+    1518         898 :   Waypoint_t      out;
+    1519         898 :   Eigen::Vector4d diff = b.coords - a.coords;
+    1520             : 
+    1521         898 :   out.coords(0) = a.coords(0) + coeff * diff(0);
+    1522         898 :   out.coords(1) = a.coords(1) + coeff * diff(1);
+    1523         898 :   out.coords(2) = a.coords(2) + coeff * diff(2);
+    1524         898 :   out.coords(3) = radians::interp(a.coords(3), b.coords(3), coeff);
+    1525             : 
+    1526         898 :   out.stop_at = false;
+    1527             : 
+    1528        1796 :   return out;
+    1529             : }
+    1530             : 
+    1531             : //}
+    1532             : 
+    1533             : /* checkNaN() //{ */
+    1534             : 
+    1535          24 : bool MrsTrajectoryGeneration::checkNaN(const Waypoint_t& a) {
+    1536             : 
+    1537          24 :   if (!std::isfinite(a.coords(0))) {
+    1538           0 :     ROS_ERROR("NaN detected in variable \"a.coords(0)\"!!!");
+    1539           0 :     return false;
+    1540             :   }
+    1541             : 
+    1542          24 :   if (!std::isfinite(a.coords(1))) {
+    1543           0 :     ROS_ERROR("NaN detected in variable \"a.coords(1)\"!!!");
+    1544           0 :     return false;
+    1545             :   }
+    1546             : 
+    1547          24 :   if (!std::isfinite(a.coords(2))) {
+    1548           0 :     ROS_ERROR("NaN detected in variable \"a.coords(2)\"!!!");
+    1549           0 :     return false;
+    1550             :   }
+    1551             : 
+    1552          24 :   if (!std::isfinite(a.coords(3))) {
+    1553           0 :     ROS_ERROR("NaN detected in variable \"a.coords(3)\"!!!");
+    1554           0 :     return false;
+    1555             :   }
+    1556             : 
+    1557          24 :   return true;
+    1558             : }
+    1559             : 
+    1560             : //}
+    1561             : 
+    1562             : /* trajectorySrv() //{ */
+    1563             : 
+    1564           4 : bool MrsTrajectoryGeneration::trajectorySrv(const mrs_msgs::TrajectoryReference& msg) {
+    1565             : 
+    1566           8 :   mrs_msgs::TrajectoryReferenceSrv srv;
+    1567           4 :   srv.request.trajectory = msg;
+    1568             : 
+    1569           4 :   bool res = service_client_trajectory_reference_.call(srv);
+    1570             : 
+    1571           4 :   if (res) {
+    1572             : 
+    1573           4 :     if (!srv.response.success) {
+    1574           0 :       ROS_WARN("[TrajectoryGeneration]: service call for trajectory_reference returned: '%s'", srv.response.message.c_str());
+    1575             :     }
+    1576             : 
+    1577           4 :     return srv.response.success;
+    1578             : 
+    1579             :   } else {
+    1580             : 
+    1581           0 :     ROS_ERROR("[TrajectoryGeneration]: service call for trajectory_reference failed!");
+    1582             : 
+    1583           0 :     return false;
+    1584             :   }
+    1585             : }
+    1586             : 
+    1587             : //}
+    1588             : 
+    1589             : /* transformTrackerCmd() //{ */
+    1590             : 
+    1591           6 : std::optional<mrs_msgs::Path> MrsTrajectoryGeneration::transformPath(const mrs_msgs::Path& path_in, const std::string& target_frame) {
+    1592             : 
+    1593             :   // if we transform to the current control frame, which is in fact the same frame as the tracker_cmd is in
+    1594           6 :   if (target_frame == path_in.header.frame_id) {
+    1595           6 :     return path_in;
+    1596             :   }
+    1597             : 
+    1598             :   // find the transformation
+    1599           0 :   auto tf = transformer_->getTransform(path_in.header.frame_id, target_frame, path_in.header.stamp);
+    1600             : 
+    1601           0 :   if (!tf) {
+    1602           0 :     ROS_ERROR("[TrajectoryGeneration]: could not find transform from '%s' to '%s' in time %f", path_in.header.frame_id.c_str(), target_frame.c_str(),
+    1603             :               path_in.header.stamp.toSec());
+    1604           0 :     return {};
+    1605             :   }
+    1606             : 
+    1607           0 :   mrs_msgs::Path path_out = path_in;
+    1608             : 
+    1609           0 :   path_out.header.stamp    = tf.value().header.stamp;
+    1610           0 :   path_out.header.frame_id = transformer_->frame_to(tf.value());
+    1611             : 
+    1612           0 :   for (size_t i = 0; i < path_in.points.size(); i++) {
+    1613             : 
+    1614           0 :     mrs_msgs::ReferenceStamped waypoint;
+    1615             : 
+    1616           0 :     waypoint.header    = path_in.header;
+    1617           0 :     waypoint.reference = path_in.points.at(i);
+    1618             : 
+    1619           0 :     if (auto ret = transformer_->transform(waypoint, tf.value())) {
+    1620             : 
+    1621           0 :       path_out.points.at(i) = ret.value().reference;
+    1622             : 
+    1623             :     } else {
+    1624           0 :       return {};
+    1625             :     }
+    1626             :   }
+    1627             : 
+    1628           0 :   return path_out;
+    1629             : }
+    1630             : 
+    1631             : //}
+    1632             : 
+    1633             : /* overtime() //{ */
+    1634             : 
+    1635        9760 : bool MrsTrajectoryGeneration::overtime(void) {
+    1636             : 
+    1637        9760 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1638        9760 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1639             : 
+    1640        9760 :   double overtime = (ros::Time::now() - start_time_total).toSec();
+    1641             : 
+    1642        9760 :   if (overtime > (OVERTIME_SAFETY_FACTOR * max_execution_time - OVERTIME_SAFETY_OFFSET)) {
+    1643          11 :     return true;
+    1644             :   }
+    1645             : 
+    1646        9749 :   return false;
+    1647             : }
+    1648             : 
+    1649             : //}
+    1650             : 
+    1651             : /* timeLeft() //{ */
+    1652             : 
+    1653          16 : double MrsTrajectoryGeneration::timeLeft(void) {
+    1654             : 
+    1655          16 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1656          16 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1657             : 
+    1658          16 :   double current_execution_time = (ros::Time::now() - start_time_total).toSec();
+    1659             : 
+    1660          16 :   if (current_execution_time >= max_execution_time) {
+    1661           0 :     return 0;
+    1662             :   } else {
+    1663          16 :     return max_execution_time - current_execution_time;
+    1664             :   }
+    1665             : }
+    1666             : 
+    1667             : //}
+    1668             : 
+    1669             : // | ------------------------ callbacks ----------------------- |
+    1670             : 
+    1671             : /* callbackPath() //{ */
+    1672             : 
+    1673           1 : void MrsTrajectoryGeneration::callbackPath(const mrs_msgs::Path::ConstPtr msg) {
+    1674             : 
+    1675           1 :   if (!is_initialized_) {
+    1676           0 :     return;
+    1677             :   }
+    1678             : 
+    1679             :   /* preconditions //{ */
+    1680             : 
+    1681           1 :   if (!sh_constraints_.hasMsg()) {
+    1682           0 :     std::stringstream ss;
+    1683           0 :     ss << "missing constraints";
+    1684           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1685           0 :     return;
+    1686             :   }
+    1687             : 
+    1688           1 :   if (!sh_control_manager_diag_.hasMsg()) {
+    1689           0 :     std::stringstream ss;
+    1690           0 :     ss << "missing control manager diagnostics";
+    1691           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1692           0 :     return;
+    1693             :   }
+    1694             : 
+    1695           1 :   if (!sh_uav_state_.hasMsg()) {
+    1696           0 :     std::stringstream ss;
+    1697           0 :     ss << "missing UAV state";
+    1698           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1699           0 :     return;
+    1700             :   }
+    1701             : 
+    1702             :   //}
+    1703             : 
+    1704             :   {
+    1705           2 :     std::scoped_lock lock(mutex_start_time_total_);
+    1706             : 
+    1707           1 :     start_time_total_ = ros::Time::now();
+    1708             :   }
+    1709             : 
+    1710           1 :   double path_time_offset = 0;
+    1711             : 
+    1712           1 :   if (msg->header.stamp != ros::Time(0)) {
+    1713           0 :     path_time_offset = (msg->header.stamp - ros::Time::now()).toSec();
+    1714             :   }
+    1715             : 
+    1716           1 :   if (path_time_offset > 1e-3) {
+    1717             : 
+    1718           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1719             : 
+    1720           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_execution_time);
+    1721             : 
+    1722           0 :     ROS_INFO("[TrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1723             :              path_time_offset);
+    1724             :   } else {
+    1725             : 
+    1726           1 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1727             : 
+    1728           1 :     max_execution_time_ = params_.max_execution_time;
+    1729             :   }
+    1730             : 
+    1731           1 :   ROS_INFO("[TrajectoryGeneration]: got path from message");
+    1732             : 
+    1733           1 :   ph_original_path_.publish(msg);
+    1734             : 
+    1735           1 :   if (msg->points.empty()) {
+    1736           0 :     std::stringstream ss;
+    1737           0 :     ss << "received an empty message";
+    1738           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1739           0 :     return;
+    1740             :   }
+    1741             : 
+    1742           2 :   auto transformed_path = transformPath(*msg, "");
+    1743             : 
+    1744           1 :   if (!transformed_path) {
+    1745           0 :     std::stringstream ss;
+    1746           0 :     ss << "could not transform the path to the current control frame";
+    1747           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1748           0 :     return;
+    1749             :   }
+    1750             : 
+    1751           1 :   fly_now_                              = transformed_path->fly_now;
+    1752           1 :   use_heading_                          = transformed_path->use_heading;
+    1753           1 :   frame_id_                             = transformed_path->header.frame_id;
+    1754           1 :   override_constraints_                 = transformed_path->override_constraints;
+    1755           1 :   loop_                                 = transformed_path->loop;
+    1756           1 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    1757           1 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    1758           1 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    1759           1 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    1760           1 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    1761           1 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    1762           1 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    1763             : 
+    1764           1 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+    1765             : 
+    1766           1 :   if (transformed_path->max_execution_time > 0) {
+    1767             : 
+    1768           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1769             : 
+    1770           0 :     max_execution_time_ = transformed_path->max_execution_time;
+    1771             : 
+    1772             :   } else {
+    1773             : 
+    1774           1 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1775             : 
+    1776           1 :     max_execution_time_ = params.max_execution_time;
+    1777             :   }
+    1778             : 
+    1779           1 :   if (transformed_path->max_deviation_from_path > 0) {
+    1780           0 :     trajectory_max_segment_deviation_ = transformed_path->max_deviation_from_path;
+    1781             :   } else {
+    1782           1 :     trajectory_max_segment_deviation_ = params.max_deviation;
+    1783             :   }
+    1784             : 
+    1785           1 :   dont_prepend_initial_condition_ = transformed_path->dont_prepend_current_state;
+    1786             : 
+    1787           1 :   std::vector<Waypoint_t> waypoints;
+    1788             : 
+    1789           5 :   for (size_t i = 0; i < transformed_path->points.size(); i++) {
+    1790             : 
+    1791           4 :     double x       = transformed_path->points.at(i).position.x;
+    1792           4 :     double y       = transformed_path->points.at(i).position.y;
+    1793           4 :     double z       = transformed_path->points.at(i).position.z;
+    1794           4 :     double heading = transformed_path->points.at(i).heading;
+    1795             : 
+    1796           4 :     Waypoint_t wp;
+    1797           4 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    1798           4 :     wp.stop_at = stop_at_waypoints_;
+    1799             : 
+    1800           4 :     if (!checkNaN(wp)) {
+    1801           0 :       ROS_ERROR("[TrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    1802           0 :       return;
+    1803             :     }
+    1804             : 
+    1805           4 :     waypoints.push_back(wp);
+    1806             :   }
+    1807             : 
+    1808           1 :   if (loop_) {
+    1809           0 :     waypoints.push_back(waypoints.at(0));
+    1810             :   }
+    1811             : 
+    1812           1 :   bool                          success = false;
+    1813           2 :   std::string                   message;
+    1814           2 :   mrs_msgs::TrajectoryReference trajectory;
+    1815             : 
+    1816           2 :   for (int i = 0; i < _n_attempts_; i++) {
+    1817             : 
+    1818             :     // the last iteration and the fallback sampling is enabled
+    1819           2 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    1820             : 
+    1821           2 :     std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading);
+    1822             : 
+    1823           2 :     if (success) {
+    1824           1 :       break;
+    1825             :     } else {
+    1826           1 :       if (i < _n_attempts_) {
+    1827           1 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    1828             :       } else {
+    1829           0 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    1830             :       }
+    1831             :     }
+    1832             :   }
+    1833             : 
+    1834           1 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    1835             : 
+    1836           1 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1837             : 
+    1838           1 :   if (total_time > max_execution_time) {
+    1839           0 :     ROS_ERROR("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    1840             :               total_time - max_execution_time);
+    1841             :   } else {
+    1842           1 :     ROS_INFO("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    1843             :   }
+    1844             : 
+    1845           1 :   trajectory.input_id = transformed_path->input_id;
+    1846             : 
+    1847           1 :   if (success) {
+    1848             : 
+    1849           1 :     bool published = trajectorySrv(trajectory);
+    1850             : 
+    1851           1 :     if (published) {
+    1852             : 
+    1853           1 :       ROS_INFO("[TrajectoryGeneration]: trajectory successfully published");
+    1854             : 
+    1855             :     } else {
+    1856             : 
+    1857           0 :       ROS_ERROR("[TrajectoryGeneration]: could not publish the trajectory");
+    1858             :     }
+    1859             : 
+    1860             :   } else {
+    1861             : 
+    1862           0 :     ROS_ERROR("[TrajectoryGeneration]: failed to calculate a feasible trajectory, no publishing a result");
+    1863             :   }
+    1864             : }
+    1865             : 
+    1866             : //}
+    1867             : 
+    1868             : /* callbackPathSrv() //{ */
+    1869             : 
+    1870           3 : bool MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res) {
+    1871             : 
+    1872           3 :   if (!is_initialized_) {
+    1873           0 :     return false;
+    1874             :   }
+    1875             : 
+    1876             :   /* preconditions //{ */
+    1877             : 
+    1878           3 :   if (!sh_constraints_.hasMsg()) {
+    1879           0 :     std::stringstream ss;
+    1880           0 :     ss << "missing constraints";
+    1881           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1882             : 
+    1883           0 :     res.message = ss.str();
+    1884           0 :     res.success = false;
+    1885           0 :     return true;
+    1886             :   }
+    1887             : 
+    1888           3 :   if (!sh_control_manager_diag_.hasMsg()) {
+    1889           0 :     std::stringstream ss;
+    1890           0 :     ss << "missing control manager diagnostics";
+    1891           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1892             : 
+    1893           0 :     res.message = ss.str();
+    1894           0 :     res.success = false;
+    1895           0 :     return true;
+    1896             :   }
+    1897             : 
+    1898           3 :   if (!sh_uav_state_.hasMsg()) {
+    1899           0 :     std::stringstream ss;
+    1900           0 :     ss << "missing UAV state";
+    1901           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1902             : 
+    1903           0 :     res.message = ss.str();
+    1904           0 :     res.success = false;
+    1905           0 :     return true;
+    1906             :   }
+    1907             : 
+    1908             :   //}
+    1909             : 
+    1910             :   {
+    1911           6 :     std::scoped_lock lock(mutex_start_time_total_);
+    1912             : 
+    1913           3 :     start_time_total_ = ros::Time::now();
+    1914             :   }
+    1915             : 
+    1916           3 :   double path_time_offset = 0;
+    1917             : 
+    1918           3 :   if (req.path.header.stamp != ros::Time(0)) {
+    1919           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    1920             :   }
+    1921             : 
+    1922           3 :   if (path_time_offset > 1e-3) {
+    1923             : 
+    1924           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1925             : 
+    1926           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_execution_time);
+    1927             : 
+    1928           0 :     ROS_INFO("[TrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1929             :              path_time_offset);
+    1930             :   } else {
+    1931             : 
+    1932           3 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1933             : 
+    1934           3 :     max_execution_time_ = params_.max_execution_time;
+    1935             :   }
+    1936             : 
+    1937           3 :   ROS_INFO("[TrajectoryGeneration]: got path from service");
+    1938             : 
+    1939           3 :   ph_original_path_.publish(req.path);
+    1940             : 
+    1941           3 :   if (req.path.points.empty()) {
+    1942           0 :     std::stringstream ss;
+    1943           0 :     ss << "received an empty message";
+    1944           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1945             : 
+    1946           0 :     res.message = ss.str();
+    1947           0 :     res.success = false;
+    1948           0 :     return true;
+    1949             :   }
+    1950             : 
+    1951           9 :   auto transformed_path = transformPath(req.path, "");
+    1952             : 
+    1953           3 :   if (!transformed_path) {
+    1954           0 :     std::stringstream ss;
+    1955           0 :     ss << "could not transform the path to the current control frame";
+    1956           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1957             : 
+    1958           0 :     res.message = ss.str();
+    1959           0 :     res.success = false;
+    1960           0 :     return true;
+    1961             :   }
+    1962             : 
+    1963           3 :   fly_now_                              = transformed_path->fly_now;
+    1964           3 :   use_heading_                          = transformed_path->use_heading;
+    1965           3 :   frame_id_                             = transformed_path->header.frame_id;
+    1966           3 :   override_constraints_                 = transformed_path->override_constraints;
+    1967           3 :   loop_                                 = transformed_path->loop;
+    1968           3 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    1969           3 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    1970           3 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    1971           3 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    1972           3 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    1973           3 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    1974           3 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    1975             : 
+    1976           6 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+    1977             : 
+    1978           3 :   if (transformed_path->max_execution_time > 0) {
+    1979             : 
+    1980           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1981             : 
+    1982           0 :     max_execution_time_ = transformed_path->max_execution_time;
+    1983             : 
+    1984             :   } else {
+    1985             : 
+    1986           3 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1987             : 
+    1988           3 :     max_execution_time_ = params.max_execution_time;
+    1989             :   }
+    1990             : 
+    1991           3 :   if (transformed_path->max_deviation_from_path > 0) {
+    1992           0 :     trajectory_max_segment_deviation_ = transformed_path->max_deviation_from_path;
+    1993             :   } else {
+    1994           3 :     trajectory_max_segment_deviation_ = params.max_deviation;
+    1995             :   }
+    1996             : 
+    1997           3 :   dont_prepend_initial_condition_ = transformed_path->dont_prepend_current_state;
+    1998             : 
+    1999           6 :   std::vector<Waypoint_t> waypoints;
+    2000             : 
+    2001          15 :   for (size_t i = 0; i < req.path.points.size(); i++) {
+    2002             : 
+    2003          12 :     double x       = transformed_path->points.at(i).position.x;
+    2004          12 :     double y       = transformed_path->points.at(i).position.y;
+    2005          12 :     double z       = transformed_path->points.at(i).position.z;
+    2006          12 :     double heading = transformed_path->points.at(i).heading;
+    2007             : 
+    2008          12 :     Waypoint_t wp;
+    2009          12 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    2010          12 :     wp.stop_at = stop_at_waypoints_;
+    2011             : 
+    2012          12 :     if (!checkNaN(wp)) {
+    2013           0 :       ROS_ERROR("[TrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    2014           0 :       res.success = false;
+    2015           0 :       res.message = "invalid path";
+    2016           0 :       return true;
+    2017             :     }
+    2018             : 
+    2019          12 :     waypoints.push_back(wp);
+    2020             :   }
+    2021             : 
+    2022           3 :   if (loop_) {
+    2023           0 :     waypoints.push_back(waypoints.at(0));
+    2024             :   }
+    2025             : 
+    2026           3 :   bool                          success = false;
+    2027           6 :   std::string                   message;
+    2028           3 :   mrs_msgs::TrajectoryReference trajectory;
+    2029             : 
+    2030           5 :   for (int i = 0; i < _n_attempts_; i++) {
+    2031             : 
+    2032             :     // the last iteration and the fallback sampling is enabled
+    2033           5 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2034             : 
+    2035           5 :     std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading);
+    2036             : 
+    2037           5 :     if (success) {
+    2038           3 :       break;
+    2039             :     } else {
+    2040           2 :       if (i < _n_attempts_) {
+    2041           2 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2042             :       } else {
+    2043           0 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2044             :       }
+    2045             :     }
+    2046             :   }
+    2047             : 
+    2048           3 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2049             : 
+    2050           3 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2051             : 
+    2052           3 :   if (total_time > max_execution_time) {
+    2053           1 :     ROS_ERROR("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2054             :               total_time - max_execution_time);
+    2055             :   } else {
+    2056           2 :     ROS_INFO("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2057             :   }
+    2058             : 
+    2059           3 :   trajectory.input_id = transformed_path->input_id;
+    2060             : 
+    2061           3 :   if (success) {
+    2062             : 
+    2063           3 :     bool published = trajectorySrv(trajectory);
+    2064             : 
+    2065           3 :     if (published) {
+    2066             : 
+    2067           3 :       res.success = success;
+    2068           3 :       res.message = message;
+    2069             : 
+    2070             :     } else {
+    2071             : 
+    2072           0 :       std::stringstream ss;
+    2073           0 :       ss << "could not publish the trajectory";
+    2074             : 
+    2075           0 :       res.success = false;
+    2076           0 :       res.message = ss.str();
+    2077             : 
+    2078           0 :       ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+    2079             :     }
+    2080             : 
+    2081             :   } else {
+    2082             : 
+    2083           0 :     ROS_ERROR("[TrajectoryGeneration]: failed to calculate a feasible trajectory, not publishing a result");
+    2084             : 
+    2085           0 :     res.success = success;
+    2086           0 :     res.message = message;
+    2087             :   }
+    2088             : 
+    2089           3 :   return true;
+    2090             : }
+    2091             : 
+    2092             : //}
+    2093             : 
+    2094             : /* callbackGetPathSrv() //{ */
+    2095             : 
+    2096           2 : bool MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res) {
+    2097             : 
+    2098           2 :   if (!is_initialized_) {
+    2099           0 :     return false;
+    2100             :   }
+    2101             : 
+    2102             :   /* preconditions //{ */
+    2103             : 
+    2104           2 :   if (!sh_constraints_.hasMsg()) {
+    2105           0 :     std::stringstream ss;
+    2106           0 :     ss << "missing constraints";
+    2107           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2108             : 
+    2109           0 :     res.message = ss.str();
+    2110           0 :     res.success = false;
+    2111           0 :     return true;
+    2112             :   }
+    2113             : 
+    2114           2 :   if (!sh_control_manager_diag_.hasMsg()) {
+    2115           0 :     std::stringstream ss;
+    2116           0 :     ss << "missing control manager diagnostics";
+    2117           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2118             : 
+    2119           0 :     res.message = ss.str();
+    2120           0 :     res.success = false;
+    2121           0 :     return true;
+    2122             :   }
+    2123             : 
+    2124           2 :   if (!sh_uav_state_.hasMsg()) {
+    2125           0 :     std::stringstream ss;
+    2126           0 :     ss << "missing UAV state";
+    2127           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2128             : 
+    2129           0 :     res.message = ss.str();
+    2130           0 :     res.success = false;
+    2131           0 :     return true;
+    2132             :   }
+    2133             : 
+    2134             :   //}
+    2135             : 
+    2136             :   {
+    2137           4 :     std::scoped_lock lock(mutex_start_time_total_);
+    2138             : 
+    2139           2 :     start_time_total_ = ros::Time::now();
+    2140             :   }
+    2141             : 
+    2142           2 :   double path_time_offset = 0;
+    2143             : 
+    2144           2 :   if (req.path.header.stamp != ros::Time(0)) {
+    2145           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    2146             :   }
+    2147             : 
+    2148           2 :   if (path_time_offset > 1e-3) {
+    2149             : 
+    2150           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2151             : 
+    2152           0 :     max_execution_time_ = FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset;
+    2153             : 
+    2154           0 :     ROS_INFO("[TrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    2155             :              path_time_offset);
+    2156             :   } else {
+    2157             : 
+    2158           2 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    2159             : 
+    2160           2 :     max_execution_time_ = params_.max_execution_time;
+    2161             :   }
+    2162             : 
+    2163           2 :   ROS_INFO("[TrajectoryGeneration]: got path from service");
+    2164             : 
+    2165           2 :   ph_original_path_.publish(req.path);
+    2166             : 
+    2167           2 :   if (req.path.points.empty()) {
+    2168           0 :     std::stringstream ss;
+    2169           0 :     ss << "received an empty message";
+    2170           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2171             : 
+    2172           0 :     res.message = ss.str();
+    2173           0 :     res.success = false;
+    2174           0 :     return true;
+    2175             :   }
+    2176             : 
+    2177           6 :   auto transformed_path = transformPath(req.path, "");
+    2178             : 
+    2179           2 :   if (!transformed_path) {
+    2180           0 :     std::stringstream ss;
+    2181           0 :     ss << "could not transform the path to the current control frame";
+    2182           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2183             : 
+    2184           0 :     res.message = ss.str();
+    2185           0 :     res.success = false;
+    2186           0 :     return true;
+    2187             :   }
+    2188             : 
+    2189           2 :   fly_now_                              = transformed_path->fly_now;
+    2190           2 :   use_heading_                          = transformed_path->use_heading;
+    2191           2 :   frame_id_                             = transformed_path->header.frame_id;
+    2192           2 :   override_constraints_                 = transformed_path->override_constraints;
+    2193           2 :   loop_                                 = transformed_path->loop;
+    2194           2 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    2195           2 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    2196           2 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    2197           2 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    2198           2 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    2199           2 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    2200           2 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    2201             : 
+    2202           4 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+    2203             : 
+    2204           2 :   if (transformed_path->max_execution_time > 0) {
+    2205             : 
+    2206           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2207             : 
+    2208           0 :     max_execution_time_ = transformed_path->max_execution_time;
+    2209             : 
+    2210             :   } else {
+    2211             : 
+    2212           2 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2213             : 
+    2214           2 :     max_execution_time_ = params.max_execution_time;
+    2215             :   }
+    2216             : 
+    2217           2 :   if (transformed_path->max_deviation_from_path > 0) {
+    2218           0 :     trajectory_max_segment_deviation_ = transformed_path->max_deviation_from_path;
+    2219             :   } else {
+    2220           2 :     trajectory_max_segment_deviation_ = params.max_deviation;
+    2221             :   }
+    2222             : 
+    2223           2 :   dont_prepend_initial_condition_ = transformed_path->dont_prepend_current_state;
+    2224             : 
+    2225           4 :   std::vector<Waypoint_t> waypoints;
+    2226             : 
+    2227          10 :   for (size_t i = 0; i < transformed_path->points.size(); i++) {
+    2228             : 
+    2229           8 :     double x       = transformed_path->points.at(i).position.x;
+    2230           8 :     double y       = transformed_path->points.at(i).position.y;
+    2231           8 :     double z       = transformed_path->points.at(i).position.z;
+    2232           8 :     double heading = transformed_path->points.at(i).heading;
+    2233             : 
+    2234           8 :     Waypoint_t wp;
+    2235           8 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    2236           8 :     wp.stop_at = stop_at_waypoints_;
+    2237             : 
+    2238           8 :     if (!checkNaN(wp)) {
+    2239           0 :       ROS_ERROR("[TrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    2240           0 :       res.success = false;
+    2241           0 :       res.message = "invalid path";
+    2242           0 :       return true;
+    2243             :     }
+    2244             : 
+    2245           8 :     waypoints.push_back(wp);
+    2246             :   }
+    2247             : 
+    2248           2 :   if (loop_) {
+    2249           0 :     waypoints.push_back(waypoints.at(0));
+    2250             :   }
+    2251             : 
+    2252           2 :   bool                          success = false;
+    2253           4 :   std::string                   message;
+    2254           2 :   mrs_msgs::TrajectoryReference trajectory;
+    2255             : 
+    2256           4 :   for (int i = 0; i < _n_attempts_; i++) {
+    2257             : 
+    2258             :     // the last iteration and the fallback sampling is enabled
+    2259           4 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2260             : 
+    2261           4 :     std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading);
+    2262             : 
+    2263           4 :     if (success) {
+    2264           2 :       break;
+    2265             :     } else {
+    2266           2 :       if (i < _n_attempts_) {
+    2267           2 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2268             :       } else {
+    2269           0 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2270             :       }
+    2271             :     }
+    2272             :   }
+    2273             : 
+    2274           2 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2275             : 
+    2276           2 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2277             : 
+    2278           2 :   if (total_time > max_execution_time) {
+    2279           0 :     ROS_ERROR("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2280             :               total_time - max_execution_time);
+    2281             :   } else {
+    2282           2 :     ROS_INFO("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2283             :   }
+    2284             : 
+    2285           2 :   if (success) {
+    2286             : 
+    2287           6 :     std::optional<geometry_msgs::TransformStamped> tf_traj_state = transformer_->getTransform("", req.path.header.frame_id, ros::Time::now());
+    2288             : 
+    2289           4 :     std::stringstream ss;
+    2290             : 
+    2291           2 :     if (!tf_traj_state) {
+    2292           0 :       ss << "could not create TF transformer for the trajectory to the requested frame";
+    2293           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2294           0 :       res.success = false;
+    2295           0 :       res.message = ss.str();
+    2296             :     }
+    2297             : 
+    2298           2 :     trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    2299             : 
+    2300         271 :     for (unsigned long i = 0; i < trajectory.points.size(); i++) {
+    2301             : 
+    2302         538 :       mrs_msgs::ReferenceStamped trajectory_point;
+    2303         269 :       trajectory_point.header    = trajectory.header;
+    2304         269 :       trajectory_point.reference = trajectory.points.at(i);
+    2305             : 
+    2306         538 :       auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    2307             : 
+    2308         269 :       if (!ret) {
+    2309             : 
+    2310           0 :         ss << "trajectory cannnot be transformed to the requested frame";
+    2311           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2312           0 :         res.success = false;
+    2313           0 :         res.message = ss.str();
+    2314             : 
+    2315             :       } else {
+    2316             : 
+    2317             :         // transform the points in the trajectory to the current frame
+    2318         269 :         trajectory.points.at(i) = ret.value().reference;
+    2319             :       }
+    2320             :     }
+    2321             : 
+    2322           2 :     res.trajectory = trajectory;
+    2323           2 :     res.success    = success;
+    2324           2 :     res.message    = message;
+    2325             : 
+    2326             :   } else {
+    2327             : 
+    2328           0 :     ROS_ERROR("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2329             : 
+    2330           0 :     res.success = success;
+    2331           0 :     res.message = message;
+    2332             :   }
+    2333             : 
+    2334           2 :   return true;
+    2335             : }
+    2336             : 
+    2337             : //}
+    2338             : 
+    2339             : /* callbackUavState() //{ */
+    2340             : 
+    2341      158510 : void MrsTrajectoryGeneration::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    2342             : 
+    2343      158510 :   if (!is_initialized_) {
+    2344           0 :     return;
+    2345             :   }
+    2346             : 
+    2347      158510 :   ROS_INFO_ONCE("[TrajectoryGeneration]: getting uav state");
+    2348             : 
+    2349      158510 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    2350             : }
+    2351             : 
+    2352             : //}
+    2353             : 
+    2354             : /* //{ callbackDrs() */
+    2355             : 
+    2356          94 : void MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, [[maybe_unused]] uint32_t level) {
+    2357             : 
+    2358          94 :   mrs_lib::set_mutexed(mutex_params_, params, params_);
+    2359             : 
+    2360             :   {
+    2361          94 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2362             : 
+    2363          94 :     max_execution_time_ = params.max_execution_time;
+    2364             :   }
+    2365             : 
+    2366          94 :   ROS_INFO("[TrajectoryGeneration]: DRS updated");
+    2367          94 : }
+    2368             : 
+    2369             : //}
+    2370             : 
+    2371             : }  // namespace mrs_uav_trajectory_generation
+    2372             : 
+    2373             : #include <pluginlib/class_list_macros.h>
+    2374          94 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trajectory_generation::MrsTrajectoryGeneration, nodelet::Nodelet);
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html new file mode 100644 index 0000000000..a67dfe05d8 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html @@ -0,0 +1,614 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.png b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..09faa278ef72510469eef301024665e9c531b94d GIT binary patch literal 7279 zcmV-#9FXIQP)sA|fK*9?RqLxc~fj z8`XaBS)`8^$^_T|g@8@0N*-Yo0^SFbK8Nc)JXSCwKsm+=cR6VVkBZJn#+ql6#z_1^ z2FUkX+!)ej_c*|U$tuPnJhFl;NlO7m7&SbyT;_l>j2hAk9vLPHKrzM|r-F8d$CDfc zj3Z2_$2bmEz_=VdE(7r%i}erAw(fQVj^gpYH0QPhlrSkKJ!J^Tj^V?18}!(+z2t6K z320flw*Z$Q1r=b8M;@cL*KI(#priC!IuppY)cbx~{|d_E^^s5?5^y+WZ_S`qFtY-4 zc(?W?{?JESJSHFBO6g&<<{G(o-;^8#;6d&X)SBpNA62`g(7{IfC$X$&Dn1R*+(LXf+lLR@yLJTF~17VDK^96+j zyx)X?54R#q0PiA<_nVCL{YC+bTU@!2rp4u}Pt9&%dM&Jv|A-We(e7-c_u)y@`HtZ( z^SUXFz*V>h(+fW8oY$DUlj4LtF5&+qf-WA34`Hu^ILpo;CF$sr-)cHx*M2|pNUt_a zw4DTBCp8W6=tBp0%H?hF7}%tFJjW#)-Nj_+A-s1EUPh%rn*?#M0s;0|i`H9#_j2D@af7bq|k# zr6vEk596a)kYouIixiL|4G+)#*NAVqi`T+{alxq0FCU1*=wNue8o%q|@ln_t<@ypa z#&ssyFtDRJx938iFZoBM%D|4@MS#U25=pzTV4SKIHauqq0-}%0{vwQBn=|Nr+`VTS zovk6eVSYK(@fcRv=pdZpG41&qc{`#&x9yq4L4CmJObxEt32mEnh&(jQZrj!^YwMF} zH|j$Y@a7H8(lXS~&6u3FZRQl|#r+qpF-;n_3Pug-!3ScIM@C9vVu%8ql#K!Dw3* zP}Lb8ll=#y!iY6|dE{HTQyN}y3V+rQAt%T69LCD{{1es;hz;`?I-~1#npBNh2xcEU zuXUtxqbv{!2qCu!C?}nyw*HtNzSkt}vh%kmhav}5j5Qe93X?34r3Wy9N4908S-5;b z61D@Q0g6yA@1_G|)>dY$Phj+88n`vXFk-ypFb0$g!1JVol!o)@agPNwQb5P!u#xdx zDoDSdN6pA6n67!60agA{@g=NERyjZ2?0hO{*7Bp@xGu3is0-mi~Lvgo<7r-m^X z4-^Rn*g4i?o#DJAu=j$=wHCdm0n@5b0feHJ%t9jtY|m@F1!!O#;n{nPnW^f*nE4&u zL6>J4K_WfA+t=53-d@N5ukZcmKi9r~8UXyNw%gZU2Z(_3hLe@xA?;ZE2)uYF`fjBd z0jMIKJFM}?^+Z5}(!owZDe3WtB+n>_^bN&o&k!0m448XT)xFY*<>L-hLdGl~xxcLC zF-E!gXq^KT?jIGTX9V~0!RK<;HIpA{EXy4bc7OTwSWI>Cmzx%!TxdZ_jNukkIy?%= zC+^4aTMKYPW4aW`Mkz6-TrLW%=uDW=i$7~40RgbbPw|+~hI{Ij9XolpiMXXCzEP(> zl;y_Feca_!R8A{;{T_#p1Jqy)w#IDFBP_L1h&33^=FIX67I#yepD8d}CZDC(|1Q9- z^ixVeO=n>Hk<;NJZ0?HzfO3ra4jslq?Ku4WrErG7tf%=Z+)~%}_+1IO?-`8y;3o4& zt&bZqzDNf40&q`&;kyi@nsn&K55m%wR$_c5)@p!qjJsuU_u@Ui``Ib%C7>81IoR0; zF9?{A%plSL?e0F`doV^U`~ID^-T$M$qN zIey2^=|~gDs5q=?oM|q?1-sMUmkk^2 z_RlNM88B5pdaRv9+H$m#f-x}X(Uf5vhmksKr2ts~IE2!%4g1|N!=^P8=P2$z$8H~aKt-=@-&}_)U4#qVu`xVP`WX_J!ig{km=?Gf zcE+<+Y|2zfYk18%s{{@+neS24?ja-7XR?D?_@m=&{)&P$?I`TtN3ia-wK#J3I@sJL zbR}F)E$=(Q@Ky|o#UR%W0{UAF3kJ&&g3jRGN2VQ`y#mO12Sbnur7&pK$s)JNA+Hw6HjGHG^4p z!^I0q3 z0K@p!NpEyR{!r#m8+07{oNF?*GxHaWcIA@G~bH^_^VDFk{Rg*n7-uVpSj9w zoKj5>RbWg99#xFo z>Gi&YtoN4<76iSjr&QdI2vjYZAp-glF-(RV=*&BBRr+1NUf?!~nz%cUb@R+oB|Vbt z5Qc9+iq4s0y zB5guF01au0Qph!&B5g_*rd_M9njIv^$R!Ub6_0jx^(SsXQwcz#{cEf1As?GlnyVTZ za81G~;b=yUvH$DsBf4{vZx#~cC(>*qPMZ(`0mT@ZZo8CVZ&9}O6h?zo9$ZhyP|xH> zr>5MF+gOas;c6l3%#SLi?I!|5@m0ikYb9=V19(0!L(Z1lK;XD={) zc7l!FfW)q}lD=dd>Q)7rS5ZrAL|#SA8E!4@wlfm1f+wAM6(>lmd!0z9!_N-V)qP=` z^d6E!M5MpIWS)4?$ELF|Q6F2*LYgxTXCVl@&11y4f%S|9T;0*%8l=*~NLig65HEto zTBb1o5+DmG4){=giEClRt+AL4pxlMk^GwQyyCV zppW+U$`rL~v=G+=n7zNowRkX(QqmwY9F8r?lGiDjn;1*bdnM_6x|kWW?CzK!dC)~n z8r)Z^B{iuM(?H6gzbbtXwNqg=R=l6tQ(jS-;P>;dAgw0j^7z^Q(eaq>-&PW1*_6R- z)jOU36qnVIG^oBI_Ikl_e7%ZGV(M#JxJBn=2PZz=7_-goIUck8&;b}7qS6JT0SRAe z?;!o6lTv}Y>+i+2yt8!OZLdWs!Ywv|y9Y1ORih*x@U?rr`i5Sp{g%XtM1!Lw5wH~{ zQC-QCHIBu67-w9nntOazU1t(peVq|6NFtZn0fxPHoy{}HE^ETSri{GKQ+wngWPqQO z%QdM0L(y<4tKs4ix49)9$N*ym?81o968l0qM%>xt?jk@1Mt3aB75c@1Tkc;I7aZ zo7)Gu`SDeq$8Bs7&~DB0ax2V!2Kf8@1w#)g*aCY2avZW!NrSmbQnk6 z5oZqViy_zLD%2A3doa>xO(r`{I*qYtI*gL(rA$H4^GIrtQFjKAZmL<8JpmwKdANA; zL?A*uCXpU~L}go!i6b>JCZPX^2B zTrw9ld#({XLiiC2n3sg*F|zf0O40~)LP>`x$HvyVyFNwM9VsAF5F`gYT@a)!*&zkw zI+FnotH5G$TZ|5`BlNoklL#L7#Hhc+{Mt2&J0V1pJ^P)0>_jLIXrL;(xaDI#98h^f ztnma4*s)iRkE(#gI`f7*XKQj>5NQg~eqGn}^|;PK1Lkn#gFl;%Q=9bl0Pl&18%Vkk zW&k8k77hA_BE4e3;@G8<;%6$(N5-(yL4qDBcU!aYNGpKc)g2s0gu9QGKH(|>ivv&$ z_#O>+T5jUchJJtw0-7K_)zMJM#7s}X$0W_pdMVW$7h}SENJVGzadbT_&*DJe<7dy4 zr&6RtJ_GRL@#2x=abTD`Ts~=E5V7nFFitXijXbxfc3v~59y!JY*ov?_pf#|{mKd3G zK$~3fYQkySq*UH^m~}u4wlWePU2-c!U_OtmB(tc(tMI?y#v||JV;*08faVQyPRzk8obsj31iUFi-Cd^ZN66v0%;Y|njb z-0Kx8+f&WEN)r2XY8H>yix-)5m;mos%b>cG4X^cXPd%`yh8g8Als8|zB^MupX zxo_GHxmn4rr$M_z`-8VC4V8{^=MDNpTKw;fCYnnq#McC39ylTJ;OeV+v7@W ztW1T!nbueyS0GyHzCs%PM8k&f+3=1kvQ$(&_ZCMR=>d(GuuI;Dw3-jN;}Yi@a+ z^&g4=SKX$f=yd3KAs%V3!(O>nu$9`o+CuE6+xEmXJ2!9@Kn2E_bW251)fjCoPIh_{ zor6KayL5}y+RR$7#o~ElxiGJ{szP?MYE9438&i8#%99SffdM1|MHMR2o!rKgKbkwa z;wB7VdlrxCv~|*;FD<~)x7b*V>%Oe3x4EjxwfM6`AGKT3u_c~Hu~hOsoM$ds-QvKb z*D{pcvgQ>l(yj9rkMbhk3OVBCl?VU^9%ZCMXI9GlNXM_L;1%P>5tahlSf=pg8vr@5 z9ywN!bzoerDjI{`9%FJq8OCGIrYg>KVWrngUBlN^cHZOOkk|rJ^>KNO(z3Usi_gv| zQX7KY&9N3LMXs}{W~(=*);CmSy39x|(wq&8j(nx{r1+xXI9v2O$@Qz|SrW;>Cx2_j zqiS1pughk_SGR2zxA6zG;C+QremI_{m8l@#AtBjSFixtNw(*<)#G{b(^N8SX);Lye zBn&@PiJv`AT?<2__DFraw#j)cT&;5tI zU9dpU3|^<`bHkChH^9}qsq*$RZ!aXSX0z<(O>;aUJY_QR>l| z;#{bV*C;VcIwNk`W23}qx&{D!8en$f6`n`YrrDz5BRekNv432sX~Ly}^YI>Ws7b+k zW&lj>iC)=Q!%a%#@RL#LA`wVBxhRY@;KVcuV_3S+XmKt5YKYANy6>578Nz5WRn625 z)Ic!2a5`dAHVlzLM55AG?{Y#N}+Gp|Y>Wvw`t<^iW@cqMNoVf-xr zan!?mP9HY9`RJwTNPl`BLoNXf8HB+C4X7#YO__kU3Bxq8)kY2FzZwIAtLa|O=5yIE*CGzP%{NHyFRN09)Mlj~(?SFk}lm|Phwhw*oc zV<9JHr}^_d3Q2?h?q4V$H@^gpBv>Zy_K>!Vz)|=J;ww^jyKhUgC3ib80^zhs!_J%| z?G%-HlxRkp0r5bKPvrTRS^^$L7i zb0Hv~Wm~(QN|D}HgpIzktqy45%sPVWxL&t0P73(8!(1PhMcXJJhQH<)IbX;KUfrY<}>y)_y8Mc2OY9?5V zrxmFk8NA8^m28E4eLd0_@3sa&%D;?mVkk@cn*ygwz*E}MDW2CiE2d8rPc0Is7tgEa z5`TnvCfH^vLXcUIX>V@!gbiQ>YdQ%y%YO{SYdlIj15@Thz>5N;%D-h1rCHj8uPan0 zm$=uUGuh-hUCWNFkClXWrt+K%+3zWCW-ndmk45nBN{tVkvy&^;A=2t&k@ZDSvGm05HSQEIipd5XzIZL8Dk*sty;^x9X>;dcmP&y0WxL%J)E+zxG5pcqKO-4S70%4RV zMIMFf3zs6Qge>3SrEtN@J};$~Xn(Seq%3oRNnNVt9>VeHy)&3hgWgV!l|WWI7gHgVwd-jI zaEBV+NA!Q|r)x{m{6%dMM%fiHS3@I?O5=5o+Ls)tvH2LZM*zS^(G|8MgEguVXDBlC}@ zAP9T?r$*+=eyCy;x2f3=)%*wM@w+nr;mUrvvLC?DYJL36*$*2Ncj`U{9J#~|%zn^m z_}^IK=){XM_2RE3UI^v**T(wF`M7dE@GQJ?KCYaPE9YZcuX)0-{fjvt`RgTD&W8l> zPvv~H{TU2c31i25?9DskR~8AqxCQZCStK6kl|=$EURfk=&U{~4BthW6mqo%pBD(p= zlzplwJr|W@Et49qERs5)7~=2`!!(0{+gtl_ZYydW@;5A3cq)GOkjG-C%J!DIU$F@G((|BB7jxj}$YJguTj^m>- z_6M&U{+rTEK^uUVo8I4(Qex&hAG}|x61eet4Ohgd>}vuPW6YR@jPZa5xZ~xPl<&5| zfC?VDr(oBFS%-0LD6Njiuc+eT)Whp|I{dK8N32}5iYLH^if4e8(-e_3~G2jG9oK}8^2&j4!uTJvs)Hm3l(j2)uBqX|vEKe< z^4zG`ulVFS{Q0cpxnHcgPxN=)@bo=(?n+s`k5RLbTsR1~Fh}#uyY?;=x^0>H)mU6% zXZ~1lzYrrCo0coC5bil6Lu)Bs7(9MFfiXmuU?TxV>iwfJ{{hlUS;8*mlJWom002ov JPDHLkV1mh{(iH#z literal 0 HcmV?d00001 diff --git a/ruby.png b/ruby.png new file mode 100644 index 0000000000000000000000000000000000000000..991b6d4ec9e78be165e3ef757eed1aada287364d GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^FceV#7`HfI^%F z9+AZi4BSE>%y{W;-5;PJOS+@4BLl<6e(pbstUx|nfKQ0)e^Y%R^MdiLxj>4`)5S5Q b;#P73kj=!v_*DHKNFRfztDnm{r-UW|iOwIS literal 0 HcmV?d00001 diff --git a/snow.png b/snow.png new file mode 100644 index 0000000000000000000000000000000000000000..2cdae107fceec6e7f02ac7acb4a34a82a540caa5 GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^MM!lvI6;R0X`wF|Ns97GD8ntt^-nBo-U3d c6}OTTfNUlP#;5A{K>8RwUHx3vIVCg!071?oo&W#< literal 0 HcmV?d00001 diff --git a/updown.png b/updown.png new file mode 100644 index 0000000000000000000000000000000000000000..aa56a238b3e6c435265250f9266cd1b8caba0f20 GIT binary patch literal 117 zcmeAS@N?(olHy`uVBq!ia0vp^AT}Qd8;}%R+`Ae`*?77*hG?8mPH5^{)z4*}Q$iB}huR`+ literal 0 HcmV?d00001